﻿//*************************************************************************
//面向服务的协作式智能交通系统仿真软件SoC-ITSS v2.1 alpha
//发布单位：西北工业大学--巴黎高科MINES机器人与群智能系统联合实验室
//团队负责人：张凯龙 Zhang Kailong
//团队成员：张大方、杨安胜、王敏、谢策、王雨佳、谢尘玉等
//联系我们：kl.zhang@nwpu.edu.cn
//2012-2021
//*************************************************************************

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using System.Threading.Tasks;
using CyberCarSimulation.Bean;
using CyberCarSimulation.RoadModel;
using CyberCarSimulation.Regulation;
using CyberCarSimulation.Simulation;



namespace CyberCarSimulation
{
    class MultiSCSPrioFIFO_Sche
    {
        private string ID;          //Agent的ID,此ID与路口ID相同
        private int IDNum;          //第几个Agent的Num号
        private int MaxAuthNum;     //最大授权
        private int MsgCount = 0;   //发送至车辆的消息数量

        /************************************************************************/
        /* 发出申请消息之后的车辆的数据信息                                                                     */
        /************************************************************************/
        //键值对，用于存储申请该临界区的车辆的VehicleManager信息，取用方式为<车辆的ID，车辆对应的CS消息>，其中每一条队列既是一个临界区所对应的CS消息
        private List<KeyValuePair<string, VehicleManager>>[] CS = new List<KeyValuePair<string, VehicleManager>>[5];
        //各车道的队列，存储各车道申请临界区的车辆的SuperVehicleManager信息，对应于4个车道
        private List<SuperVehicleManager>[] LaneQueue = new List<SuperVehicleManager>[5];

        /************************************************************************/
        /* 当前已授权车辆的信息，此处只存储其车辆和临界区消息，不存储具体的消息                                                                     */
        /************************************************************************/
        //键值对，<CSID，ID>用于记录将编号为CSID的S-CS资源授权给编号为ID的车辆,其数目最多为4 ，对应于4个临界区
        public Dictionary<int, string> Authorize_CS_to_Vehicle = new Dictionary<int, string>();
        //保存当前被授权的车辆ID号，其数量最多为4，代表一个时间段之内最多只有4辆车被授权  //只是部分授权，并没有全部授权
        private List<string> Authorized_Vehicle_Sequence = new List<string>();
        //保存之前已经被授权完成的车辆这些车辆无需再被授权   //已经全部授权
        private List<string> Already_Authorized_All_CS_Vehicles = new List<string>();
        //Token[XXX]为true表示编号为XXX的S-CS资源已经被占用，为false则表示该资源空闲
        private bool[] Token = new bool[5];

        //为true表示当前发出预约请求的车辆中没有高优先级车辆，为false则相反。该变量用于控制调度算法切换。
        private bool isAllLowPrior = true;
        //标志出现的优先车辆是高优先级车辆还是中优先级车辆，其中Definitions.Prio_High表示高优先级车辆，Definitions.Prio_Mid表示中优先级车辆
        private int PriorType = Definitions.Prio_High;

        //<ID，SVM>将编号为ID的车辆与对应的SuperVehicleManager对象SVM关联起来，同时也是调度器中的全局队列，存储全部车辆对S-CS资源的需求信息
        private Dictionary<string, SuperVehicleManager> VehicleMng = new Dictionary<string, SuperVehicleManager>();
        //原高优先级车辆，其中float为到达下一个临界区的时间，但是尽量不做排序
        private List<string> HighPriorVehicleOrigin = new List<string>();
        //当前原高优先级车辆所处的车道
        private List<int> HighPriorLaneIDOrigin = new List<int>();
        //以确定为高优先级车辆 = 原高优先级车辆  + 占用原高优先级车辆临界区的车辆，
        //其存储方式为<车辆ID>，最终在遍历其中的元素的时候需要将其值按照到达时间进行排序
        private List<string> HighPriorVehicle = new List<string>();
        //存储当前的排完序之后的所有高优先级 ，按照的规则是 已占用临界区的车辆 + 未占用临界区的车辆  的进行排序，
        //其中处于同一集合，如都处于已占用临界区的车辆，其先后顺序按照 到达下一临界区的先后顺序进行排序
        private List<SuperVehicleManager> HighPriorVehiclesSorted = new List<SuperVehicleManager>();

        //存储当前各临界区中能够被未造成延迟的申请该临界区的车辆
        private List<string>[] CanBeAuthorized = new List<string>[5];

        //用于分辨是否会产生死锁
        private bool[] Walked = new bool[5];
        private bool[,] SourceGraph = new bool[5, 5];

        /************************************************************************/
        /* 当临界区授权情况改变时，包括新授权和授权收回的情况，输出此时占用临界区的车辆，全局时间等信息。                                                                     */
        /************************************************************************/
        //private void OutPutAuthMsg(int mark, Message msg)
        //{
        //    if (DisplayForm.IsOutputAuthMsg)
        //    {
        //        StreamWriter sw1 = null;
        //        switch (mark)
        //        {
        //            case 1: //输出授权临界区时的信息
        //                //输出当前所有授权信息
        //                if (Directory.Exists("./Authrized") == false)
        //                {
        //                    Directory.CreateDirectory("./Authrized");
        //                }
        //                sw1 = new StreamWriter(new FileStream("./Authrized/" + "Total_" + Definitions.FileName[Main.ScheType] + Definitions.Division + DisplayForm.SimuIndex + ".txt", FileMode.Append), Encoding.ASCII);
        //                sw1.WriteLine(Main.getGlobalTime());
        //                foreach (KeyValuePair<int, string> kvp in this.Authorize_CS_to_Vehicle)
        //                {
        //                    sw1.WriteLine(kvp.Key + "\t" + kvp.Value + "\t");
        //                }
        //                sw1.WriteLine("");
        //                sw1.Close();

        //                //输出当前的被授权车辆的信息
        //                sw1 = new StreamWriter(new FileStream("./Authrized/" + Definitions.FileName[Main.ScheType] + Definitions.Division + DisplayForm.SimuIndex + ".txt", FileMode.Append), Encoding.ASCII);
        //                string ID_tmp = msg.getReceiver();
        //                string CSID_tmp = msg.getSectionID();
        //                int Priority_tmp = msg.getPriority();
        //                sw1.WriteLine(Main.getGlobalTime());
        //                sw1.WriteLine(ID_tmp + "\t" + CSID_tmp + "\t" + Priority_tmp);
        //                sw1.WriteLine();
        //                sw1.Close();
        //                break;
        //            case 2: //输出收回临界区时的信息
        //                //输出当前所有授权信息
        //                if (Directory.Exists("./Recycle") == false)
        //                {
        //                    Directory.CreateDirectory("./Recycle");
        //                }
        //                //                     sw1.WriteLine("CSID  ID");
        //                sw1 = new StreamWriter(new FileStream("./Recycle/" + "Total_" + Definitions.FileName[Main.ScheType] + Definitions.Division + DisplayForm.SimuIndex + ".txt", FileMode.Append), Encoding.ASCII);
        //                sw1.WriteLine(Main.getGlobalTime());
        //                foreach (KeyValuePair<int, string> kvp in this.Authorize_CS_to_Vehicle)
        //                {
        //                    sw1.WriteLine(kvp.Key + "\t" + kvp.Value + "\t");
        //                }
        //                sw1.WriteLine("");
        //                sw1.Close();

        //                //输出当前的被授权车辆的信息
        //                sw1 = new StreamWriter(new FileStream("./Recycle/" + Definitions.FileName[Main.ScheType] + Definitions.Division + DisplayForm.SimuIndex + ".txt", FileMode.Append), Encoding.ASCII);
        //                ID_tmp = msg.getReceiver();
        //                CSID_tmp = msg.getSectionID();
        //                Priority_tmp = msg.getPriority();
        //                sw1.WriteLine(Main.getGlobalTime());
        //                sw1.WriteLine(ID_tmp + "\t" + CSID_tmp + "\t" + Priority_tmp);
        //                sw1.WriteLine();
        //                sw1.Close();
        //                break;
        //            case 3: //输出提升优先级是的车辆信息
        //                break;
        //            default:
        //                break;
        //        }
        //    }
        //}


        /************************************************************************/
        /* 初始化                                                                     */
        /************************************************************************/
        public MultiSCSPrioFIFO_Sche(string ID)
        {
            //设置ID
            this.ID = ID;
            this.IDNum = int.Parse(ID.Substring(3, 2));

            for (int i = 0; i < 5; i++)
            {
                this.Token[i] = false;//为true表示编号为XXX的S-CS资源已经被占用，为false则表示该资源空闲
                this.CS[i] = new List<KeyValuePair<string, VehicleManager>>();
                this.LaneQueue[i] = new List<SuperVehicleManager>();
                this.CanBeAuthorized[i] = new List<string>();
            }
        }

        /************************************************************************/
        /* 验证是否会出现LaneQueue中的车辆的Priority大于this.PriorType                                                                     */
        /************************************************************************/
        private void testPrior()
        {
            for (int i = 0; i < 5; i++)
            {
                for (int j = 0; j < this.LaneQueue[i].Count; j++)
                {
                    string ID = this.LaneQueue[i].ElementAt(j).ID;
                    SuperVehicleManager stmp = this.FindPosInLaneQueue(ID);
                    if (stmp.Priority > this.PriorType)
                    {
                        System.Windows.Forms.MessageBox.Show("PriorRaised error");
                    }
                }
            }
        }

        /************************************************************************/
        /* MultiSCSPrioFIFO_Sche对象的对外接口                                                                     */
        /************************************************************************/
        public void executeAlgorithm(Intersection Intersection)
        {
            //读入消息
            this.readMessageFromVehicle(Intersection);
            this.isAllLowPrior = this.isPriorVehiOccur(); //isPriorVehiOccur()检测是否有高优先级车辆出现在预约队列中,若有高优先级车辆出现，则返回true,否则返回false
            //this.testPrior();                             //验证是否会出现LaneQueue中的车辆的Priority大于this.PriorType 

            if (this.isAllLowPrior == true)               //&& this.Authorized_Vehicle_Sequence.Count == 0)
            {
                this.OriginalHighPriorVehicleQueueCreate(this.PriorType);  //获取当前的原高优先级车辆和原高优先级车辆车道 
                this.executeAlgorithmPrior(Intersection);
            }
            else
            {
                this.executeAlgorithmFIFO(Intersection);
            }
        }

        /************************************************************************/
        /* 用于执行FIFO调度算法                                                                     */
        /************************************************************************/
        private void executeAlgorithmFIFO(Intersection Intersection)
        {
            //处理高优先级车辆
            this.processLeftVehicles(Intersection);
            this.ReSetIsSelect();
        }

        /************************************************************************/
        /* 用于执行Prio调度算法                                                                     */
        /************************************************************************/
        private void executeAlgorithmPrior(Intersection Intersection)
        {
            //被授权车辆超过4辆
            if (this.Authorized_Vehicle_Sequence.Count > 4)
            {
                System.Windows.Forms.MessageBox.Show("the num of authorized vehicle is more than 4_1");
                return;
            }
            //按照出现高优先级或者中优先级的不同情况进行分类
            if (this.PriorType == Definitions.Prio_High)
            {
                this.processPriority(Definitions.Prio_High, Intersection);
            }
            else if (this.PriorType == Definitions.Prio_Mid)
            {
                this.processPriority(Definitions.Prio_Mid, Intersection);
            }
         
            this.ReSetIsSelect();
        }


        /************************************************************************/
        /* 从Msg_Buffer类的存储空间中读取相关信息，如Msg_Reservation消息、Msg_Release消息等。                                                                     */
        /************************************************************************/
        private void readMessageFromVehicle(Intersection Intersection)
        {
            //获取路口中的消息队列            
            List<Message> VtoA_BufferTemp = Intersection.GetVtoA_Buffer();    //局部变量，需要重新写回去

            while (VtoA_BufferTemp.Count != 0)
            {
                //取出一条Message
                Message m = VtoA_BufferTemp.ElementAt(0);
                VtoA_BufferTemp.Remove(m);

                switch (m.GetType())
                {
                    //在预约消息下需要创建新消息，并将消息存入不同的队列中
                    case Definitions.MsgReservation:
                        SuperVehicleManager svm;
                        string LaneIDTemp = m.GetLaneIDStr();
                        svm = new SuperVehicleManager(m.GetSender(), int.Parse(LaneIDTemp.Substring(5, 1)), m.GetTsArray(), m.GetTeArray(),
                                            m.GetTotal_Dis(), m.GetSectionID(), m.GetPriority());
                        /*将生成的车辆管理实体加入到对应的队列中*/
                        //因为进入车道的车辆本身具有具有顺序，所以此处无需对车道按照车辆插入时间对车辆进行排序
                        this.LaneQueue[svm.LaneID].Add(svm);
                        this.updateCSQueue(svm);
                        this.sortCSQueue();
                        break;
                    case Definitions.MsgFault:
                        break;
                    case Definitions.MsgWait:
                        break;
                    case Definitions.MsgRenew:
                        //主要用于更新车辆到达时间
                        string str = m.GetLaneIDStr();  //车辆的ID
                        string section = m.GetSectionID();
                        float[] ts = m.GetTsArray();
                        float[] te = m.GetTeArray();

                        for (int i = 0; i < this.LaneQueue[int.Parse(str[5].ToString())].Count(); ++i)
                        {
                            if (this.LaneQueue[int.Parse(str[5].ToString())].ElementAt(i).getID() == m.GetSender())
                            {
                                this.LaneQueue[int.Parse(str[5].ToString())].ElementAt(i).setTs(m.GetTsArray());
                                this.LaneQueue[int.Parse(str[5].ToString())].ElementAt(i).setTe(m.GetTeArray());
                                SuperVehicleManager stmp = this.LaneQueue[int.Parse(str[5].ToString())].ElementAt(i);

                                for (int j = 0; j < section.Length; j++)
                                {
                                    if (stmp.VehicleManagers[int.Parse(section[j].ToString())] != null)
                                    {
                                        stmp.VehicleManagers[int.Parse(section[j].ToString())].setTs(ts[j]);
                                        stmp.VehicleManagers[int.Parse(section[j].ToString())].setTe(te[j]);
                                    }
                                }
                            }
                        }

                        for (int j = 0; j < section.Length; j++)
                        {
                            foreach (KeyValuePair<string, VehicleManager> kvp in this.CS[int.Parse(section[j].ToString())])
                            {
                                if (kvp.Key == m.GetSender())
                                {
                                    kvp.Value.setTs(ts[j + 1]);
                                    kvp.Value.setTe(te[j + 1]);
                                }
                            }
                        }
                        this.sortCSQueue();
                        break;
                    case Definitions.MsgRelease:
                        //将对应的临界区的消息去除
                        str = m.GetSender();

                        Vehicle VehicleTemp = FindVehicleByID(str);
                        string Lanestr = VehicleTemp.Get_Previous_LaneID();

                        //CS信息，若释放，则删除对应的临界区
                        foreach (KeyValuePair<string, VehicleManager> kvp in this.CS[int.Parse(m.GetSectionID().ToString())])
                        {
                            if (kvp.Key == m.GetSender())
                            {
                                this.CS[int.Parse(m.GetSectionID().ToString())].Remove(kvp);
                                break;
                            }
                        }
                        //LaneQueue信息，若释放，则将对应的临界区的token信息，left信息更新，其中HaveTokenCount因为其已被申请过，所以不作修改
                        for (int i = 0; i < this.LaneQueue[int.Parse(str[5].ToString())].Count(); ++i)
                        {
                            if (this.LaneQueue[int.Parse(str[5].ToString())].ElementAt(i).getID() == m.GetSender())
                            {
                                this.LaneQueue[int.Parse(str[5].ToString())].ElementAt(i).Token[int.Parse(m.GetSectionID().ToString())] = false;
                                this.LaneQueue[int.Parse(str[5].ToString())].ElementAt(i).Left[int.Parse(m.GetSectionID().ToString())] = true;
                            }
                        }
                   
                        //将被占用临界区标志位为未被占用
                        this.Token[int.Parse(m.GetSectionID().ToString())] = false;
                        this.Authorize_CS_to_Vehicle.Remove(int.Parse(m.GetSectionID().ToString()));
                        //this.OutPutAuthMsg(2, m);
                        break;
                    case Definitions.MsgPassing:
                        //标志车辆正在经过某临界区，无需处理
                        break;
                    case Definitions.MsgPassed:
                        //车辆离开十字路口
                        this.removePassedVehicle(m.GetSender());
                        break;
                    default:
                        System.Windows.Forms.MessageBox.Show("Unknown Message Type ------ " + m.GetType(), "Error", System.Windows.Forms.MessageBoxButtons.OK, System.Windows.Forms.MessageBoxIcon.Error);
                        break;
                }
            }
            //将数据写会MainData中
            Intersection.SetVtoA_Buffer(VtoA_BufferTemp);
            MainData.UpdateIntersectionList(Intersection);
        }

        /************************************************************************/
        /* 更新个临界区的消息队列                                                                     */
        /************************************************************************/
        private void updateCSQueue(SuperVehicleManager svm)
        {
            for (int i = 1; i < 5; i++)
            {
                if (svm.VehicleManagers[i] != null)
                {
                    VehicleManager vm = svm.VehicleManagers[i];
                    VehicleManager vm2 = new VehicleManager(vm.getID(), vm.getLaneID(), vm.getTs(), vm.getTe(),
                        vm.getCurrent_Dis(), vm.getSectionIDs(), vm.getOriginalPriority(), true);
                    vm2.setCSCount(svm.VehicleManagers[i].getCSCount());

                    //更新临界区的消息队列
                    KeyValuePair<string, VehicleManager> kvp = new KeyValuePair<string, VehicleManager>(vm2.getID(), vm2);
                    this.CS[int.Parse(vm2.getSectionIDs())].Add(kvp);
                }
            }
        }

        /************************************************************************/
        /* 将各临界区的申请车辆按照到达时间的先后顺序进行排序                                                                     */
        /************************************************************************/
        private void sortCSQueue()
        {
            for (int i = 1; i < 5; i++)
            {
                if (this.CS[i].Count != 0)
                {
                    for (int j = 0; j < this.CS[i].Count; j++)
                    {
                        for (int k = j + 1; k < this.CS[i].Count; k++)
                        {
                            if (this.CS[i].ElementAt(j).Value.getTs() > this.CS[i].ElementAt(k).Value.getTs())
                            {
                                KeyValuePair<string, VehicleManager> kvp;
                                kvp = this.CS[i].ElementAt(j);
                                this.CS[i][j] = this.CS[i].ElementAt(k);
                                this.CS[i][k] = kvp;
                            }
                        }
                    }
                }
            }
        }

        /************************************************************************/
        /* 将过了临界区的车辆删除，并释放占用的资源,
         * ID为车辆的编号                                                                     */
        /************************************************************************/
        private void removePassedVehicle(string ID)
        {
            //int LaneID = int.Parse(ID[0].ToString());
            int LaneID = FindLaneIDByVehicleID(ID);

            if (this.LaneQueue[LaneID].Count == 0)
            {
                return;
            }
            //删除车道车辆列表中的车辆
            foreach (SuperVehicleManager stmp in LaneQueue[LaneID])
            {
                if (stmp.ID == ID)
                {
                    this.LaneQueue[LaneID].Remove(stmp);
                    break;
                }
            }
            //将申请过临界区的车辆释放
            for (int i = 1; i < 5; i++)
            {
                foreach (KeyValuePair<string, VehicleManager> kvp in this.CS[i])
                {
                    if (kvp.Key == ID)
                    {
                        this.CS[i].Remove(kvp);
                        //                         break;
                    }
                }
            }
            //将授权过的的车辆释放
            for (int i = 0; i < Authorized_Vehicle_Sequence.Count; ++i)
            {
                if (this.Authorized_Vehicle_Sequence.ElementAt(i) == ID)
                {
                    this.Authorized_Vehicle_Sequence.RemoveAt(i);
                    break;
                }
            }
            //将授权过的临界区的释放
            for (int i = 1; i < 5; i++)
            {
                if (this.Authorize_CS_to_Vehicle.ContainsKey(i) == true)
                {
                    if (this.Authorize_CS_to_Vehicle[i] == ID)
                    {
                        this.Authorize_CS_to_Vehicle.Remove(i);
                        break;
                    }
                }
            }
            //释放HighPriorVehicleOrigin中的车辆
            if (this.HighPriorVehicleOrigin.Contains(ID))
            {
                this.HighPriorVehicleOrigin.Remove(ID);
            }
            //释放HighPriorVehicleOrigin中的车辆
            if (this.HighPriorLaneIDOrigin.Contains(LaneID))
            {
                this.HighPriorLaneIDOrigin.Remove(LaneID);
            }
            //释放HighPriorVehicle中的车辆
            if (this.HighPriorVehicle.Contains(ID))
            {
                this.HighPriorVehicle.Remove(ID);
            }
            //释放HighPriorVehiclesSorted中的车辆
            foreach (SuperVehicleManager stmp in this.HighPriorVehiclesSorted)
            {
                if (stmp.ID == ID)
                {
                    this.HighPriorVehiclesSorted.Remove(stmp);
                    break;
                }
            }
            return;
        }

        /************************************************************************/
        /* 对车辆进行授权                                                                          */
        /************************************************************************/
        private void processLeftVehicles(Intersection Intersection)
        {
            string s = null;

            //被授权车辆超过4辆
            if (this.Authorized_Vehicle_Sequence.Count > 4)
            {
                System.Windows.Forms.MessageBox.Show("the num of authorized vehicle is more than 4_2");
                return;
            }

            s = null;
            if (!IsCSEmpty())
            {
                //若存在临界区未被占用
                VehicleManager vtmp = null;
                while ((vtmp = this.GetEarliestFromCSVehicle()) != null)
                {
                    //临界区分配完毕，则退出
                    if (this.Token[1] == true && this.Token[2] == true && this.Token[3] == true && this.Token[4] == true)
                    {
                        break;
                    }

                    //被选中车辆的CSID
                    int CSID = int.Parse(vtmp.getSectionIDs());
                    //被选中车辆的ID
                    string ID = vtmp.getID();

                    //找到车辆Vehicle对象的LaneID                        
                    string LaneID = FindVehicleByID(ID).Get_Current_LaneID();  


                    //临界区已被选中
                    if (this.Token[CSID] == true)
                    {
                        continue;
                    }

                    if (ID != null
                        && (this.isTokenSequenceIllegal(int.Parse(LaneID[5].ToString()), ID, CSID, ref s) == false)
                        && (this.isFirstVehicleInLane(int.Parse(LaneID[5].ToString()), ID, CSID) == true))
                    {
                       

                        this.Authorize_CS_to_Vehicle.Add(CSID, ID);
                        if (this.isTokenSequenceDeadLock(this.Authorize_CS_to_Vehicle) != true)//不会死锁
                        {                           

                            //若车辆被授予临界区，但是未被授予所有临界区，那么就将该车存入Authorized_Vehicle_Sequence，
                            if (this.Authorize_CS_to_Vehicle.Keys.Contains(CSID))
                            {
                                if (this.Authorize_CS_to_Vehicle[CSID] == ID)
                                {

                                }
                                else
                                {
                                    System.Windows.Forms.MessageBox.Show("Authorize_CS_to_Vehicle error4");
                                }
                            }
                            else
                            {
                                this.Authorize_CS_to_Vehicle.Add(CSID, ID);
                            }

                            this.sendMessage(Definitions.MsgAuthorize, ID, vtmp.getCurrentPriority(), CSID, Intersection);
                            this.setToken(CSID, ID);

                            //判断当前被授权车辆是否还有未申请的临界区
                            SuperVehicleManager stmp = null;
                            int kkkk = 0;
                            for (int i = 1; i < 5; ++i)
                            {
                                for (int j = 0; j < this.LaneQueue[i].Count; ++j)
                                {
                                    if (this.LaneQueue[i].ElementAt(j).ID == ID)
                                    {
                                        stmp = this.LaneQueue[i].ElementAt(j);
                                        for (kkkk = 0; kkkk < stmp.CS_Count; kkkk++)
                                        {
                                            if (stmp.Token[int.Parse(stmp.SectionID[kkkk].ToString())] == false)
                                            {
                                                break;
                                            }
                                        }
                                    }
                                }
                            }

                            //若车辆未申请完成所有临界区，则将其存入Authorized_Vehicle_Sequence中，作为当前仍需申请临界区的车辆对待
                            if (kkkk != vtmp.getCSCount())
                            {
                                if (stmp != null)
                                {
                                    bool mark = false;
                                    for (int k = 0; k < this.Authorized_Vehicle_Sequence.Count; k++)
                                    {
                                        if (this.Authorized_Vehicle_Sequence[k] == stmp.ID)
                                        {
                                            mark = true;
                                        }
                                    }
                                    if (mark == false)
                                    {
                                        this.Authorized_Vehicle_Sequence.Add(stmp.ID);
                                    }
                                }
                            }
                            else
                            {//该车辆已经申请完成所有的临界区，无需再次判断

                                bool mark = false;
                                for (int k = 0; k < this.Authorized_Vehicle_Sequence.Count; k++)
                                {
                                    if (this.Authorized_Vehicle_Sequence[k] == stmp.ID)
                                    {
                                        mark = true;
                                    }
                                }
                                if (mark == false)
                                {
                                    this.Authorized_Vehicle_Sequence.Add(stmp.ID);
                                }
                                this.Already_Authorized_All_CS_Vehicles.Add(stmp.ID);
                            }
                        }
                        else    //会产生死锁
                        {
                            this.Authorize_CS_to_Vehicle.Remove(CSID);
                        }
                    }
                }
            }
            
        }

        /************************************************************************/
        /* 获取各个道路列表中最早到达的车辆,用于每次先进行判断的车辆，返回车辆的ID号                                                                     */
        /************************************************************************/
        private VehicleManager GetEarliestFromCSVehicle()
        {
            VehicleManager vtmp = null;
            VehicleManager SelTmp = null;
            float EarliestTime = 99999.0f;
            int ChosenCS = 0;       //标志所选中的CS号
            int ChosenVehi = 0;     //标志所选中车道中的车辆的ID号

            for (int i = 1; i < 5; ++i)
            {
                if (this.Token[i] == false)
                {
                    for (int j = 0; j < this.CS[i].Count; ++j)
                    {
                        vtmp = this.CS[i].ElementAt(j).Value;
                        string str = vtmp.getID();
                        //车辆已经经过该临界区
                        if ((vtmp.getTs() - Main.getGlobalTime()) <= 0.00000000001f)
                        {
                            Debug.Write("fdasdfdas");
                            continue;
                        }
                        if (Already_Authorized_All_CS_Vehicles.Contains(str))
                        {
                            //车辆的所有临界区已被授权
                            Debug.Write("fdasdfdas");
                            continue;
                        }
                        else
                        {
                            //车辆未被选中,则进行车辆的选择
                            if (vtmp.isSelect == false)
                            {
                                if ((EarliestTime - vtmp.getTs()) > 0.00000000001f)
                                {
                                    ChosenCS = i;
                                    ChosenVehi = j;
                                    EarliestTime = vtmp.getTs();
                                    SelTmp = vtmp;
                                }
                            }
                        }
                    }
                }
                else
                {

                }
            }
            if (SelTmp != null)
            {
                this.CS[ChosenCS].ElementAt(ChosenVehi).Value.isSelect = true;
            }

            return SelTmp;
        }

        /************************************************************************/
        /* 检验在道路消息队列组中的某一辆车是否，判断一辆是否是按照临界区先后循序进行申请的，返回true则非法                                                                     */
        /************************************************************************/
        private bool isTokenSequenceIllegal(int LaneID, string ID, int AuthCSID, ref string RepID)
        {
            SuperVehicleManager svm = null;
            int d = 0;

            //获取道路消息队列组中需要进行判断的车辆的总消息
            for (int i = 0; i < this.LaneQueue[LaneID].Count; i++)
            {
                if (this.LaneQueue[LaneID].ElementAt(i).ID == ID)
                {
                    svm = this.LaneQueue[LaneID].ElementAt(i);
                    break;
                }
            }

            if (svm != null)
            {
                //获取当前需要判断的临界区，即将要处理的临界区
                for (int i = 0; i < svm.CS_Count; i++)
                {
                    if (svm.VehicleManagers[int.Parse(svm.SectionID[i].ToString())] == null)
                    {
                        System.Windows.Forms.MessageBox.Show("error,isTokenSequenceIllegal1");
                        continue;
                    }
                    if (int.Parse(svm.SectionID[i].ToString()) == AuthCSID)
                    {
                        d = i;
                        break;
                    }
                }

                for (int i = 0; i < d; i++)
                {
                    //消息按照临界区序号进行存储
                    if (svm.VehicleManagers[int.Parse(svm.SectionID[i].ToString())] == null)
                    {
                        System.Windows.Forms.MessageBox.Show("error,isTokenSequenceIllegal2");
                        continue;
                    }
                    //车辆的所申请临界区之前的临界区已经申请完成，且所申请临界区该临界区还未申请成功，此时则说明合法
                    if (svm.VehicleManagers[int.Parse(svm.SectionID[i].ToString())].HasToken == false)
                    {
                        //                        Debug.WriteLine("Illegal \t" + ID + "\t" + AuthCSID + "\t" + svm.SectionID);
                        RepID = ID;
                        return true;
                    }
                }
            }
            else
            {
                //test
                Debug.WriteLine("fjkafds");
                //test finish
            }
            return false;
        }

        /************************************************************************/
        /* 检验当前授权车辆是否是道路中的第一辆车,若是首车，则返回true                                                                     */
        /************************************************************************/
        private bool isFirstVehicleInLane(int LaneID, string ID, int CSID)
        {
            SuperVehicleManager ktmp;
            string scs = null;
            int k = 0;
            int k2 = 1;
            //找到正在判断的车辆
            foreach (SuperVehicleManager kvp in LaneQueue[LaneID])
            {
                if (kvp.ID == ID)
                {
                    ktmp = kvp;
                    scs = ktmp.Original_SectionID;
                    break;
                }
                ++k;
            }
            //正在判断中车辆所申请临界区在其所用临界区中所处的位置
            for (int i = 0; i < scs.Length; ++i)
            {
                if (CSID != int.Parse(scs[i].ToString()))
                {
                    ++k2;
                }
                else
                {
                    break;
                }
            }
            //判断是否是首车
            bool mark = true;   //若是首车，则mark为true
            for (int i = 0; i < k; ++i)   //遍历申请车辆之前的所有车
            {
                for (int j = 0; j < k2; ++j)  //遍历包含所申请临界区之前的所有临界区
                {
                    string pre_scs = LaneQueue[LaneID].ElementAt(i).Original_SectionID;
                    if (pre_scs.Contains(scs[j].ToString()) == true)//前车包含本车所要申请的临界区，此时需判断前车是否已经离开该临界区
                    {
                        if (LaneQueue[LaneID].ElementAt(i).Left[CSID] == true)  //前车已离开该临界区，本车已可申请该临界区
                        {

                        }
                        else
                        {
                            mark = false;
                        }
                    }
                    else    //前车未包含本车所要申请的临界区，此时无需判断前车是否已经离开该临界区
                    {

                    }
                }
            }

            if (mark == true)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        /************************************************************************/
        /* 检测资源是否出现环路，若返回true，则产生了回路 
         * 1、若该临界区被走过，则说明产生了回路
         * 2、若未被走过，则判断其所需的下一临界区
         */
        /************************************************************************/
        private bool dfsCheckCircuit(int current)
        {
            if (Walked[current])
            {
                return true;
            }
            Walked[current] = true;
            for (int i = 1; i <= 4; i++)
            {
                if (this.SourceGraph[current, i])
                {
                    //若箭头存在，即该临界区需求下一临界区
                    //则继续判断该临界区是否会出现回路
                    if (dfsCheckCircuit(i))
                    {
                        return true;
                    }
                }
            }
            Walked[current] = false;
            return false;
        }

        /************************************************************************/
        /* 判断是否会出现死锁，返回True则死锁产生                                                                     */
        /************************************************************************/
        private bool isTokenSequenceDeadLock(Dictionary<int, string> SQ)
        {
            int i, j;
            int Count = 0;
            int[] array = new int[5];
            SuperVehicleManager[] svm = new SuperVehicleManager[SQ.Count];

            for (i = 0; i < 5; i++)
            {
                for (j = 0; j < 5; j++)
                {
                    this.SourceGraph[i, j] = false;
                }
                this.Walked[i] = false;
            }

            //获取待授权的所有车辆的车辆superManager信息，存入svm中
            for (i = 0; i < SQ.Count; i++)
            {
                String VehicleID = SQ.ElementAt(i).Value;
                Vehicle VehicleTemp = FindVehicleByID(VehicleID);//通过ID找到Vehicle车辆，再通过车辆找到所在车道。即就是通过Vehicle类可以拿到车辆所在车道。
                String LaneIDTemp = VehicleTemp.Get_Current_LaneID();
                int LaneID = int.Parse(LaneIDTemp.ElementAt(5).ToString());

                for (j = 0; j < this.LaneQueue[LaneID].Count; j++)
                {
                    if (this.LaneQueue[LaneID].ElementAt(j).ID == SQ.ElementAt(i).Value)
                    {
                        svm[Count] = this.LaneQueue[LaneID].ElementAt(j);
                        Count++;
                        break;
                    }
                }
            }

            //构建资源分配表
            for (i = 0; (i + 3) < 7; ++i)
            {//临界区标号
                for (j = 0; j < Count; j++)
                {//车辆数目
                    if (int.Parse(svm[j].SectionID[i].ToString()) != 0)
                    {
                        if (svm[j].Left[int.Parse(svm[j].SectionID[i].ToString())] == false)
                        {//若该临界区未走过，则将其下面要走的路径标志为true
                            if (int.Parse(svm[j].SectionID[i + 1].ToString()) != 0)
                            {
                                this.SourceGraph[int.Parse(svm[j].SectionID[i].ToString()), int.Parse(svm[j].SectionID[i + 1].ToString())] = true;
                            }
                            if (int.Parse(svm[j].SectionID[i + 2].ToString()) != 0)
                            {
                                this.SourceGraph[int.Parse(svm[j].SectionID[i + 1].ToString()), int.Parse(svm[j].SectionID[i + 2].ToString())] = true;
                            }
                            if (int.Parse(svm[j].SectionID[i + 3].ToString()) != 0)
                            {
                                this.SourceGraph[int.Parse(svm[j].SectionID[i + 2].ToString()), int.Parse(svm[j].SectionID[i + 3].ToString())] = true;
                            }
                        }
                        else
                        {//如果车辆已离开该临界区则无需在将临界区加入继续判断是否会死锁
                            /*                            Debug.WriteLine("fdasfsd");*/
                        }
                    }
                    else
                    {//若车辆为经过该临界区

                    }
                }
            }

            //检查是否有回路出现
            for (i = 1; i <= 4; i++)
            {
                //从第一个临界区开始检测，通过递归的方式，检测其所需要的临界区是否被走过，
                //若走过，那么就会出现环路
                if (dfsCheckCircuit(i))
                {
                    return true;
                }
            }
            return false;
        }

        /************************************************************************/
        /* 发送临界区授权信息                                                                     */
        /************************************************************************/
        private void sendMessage(int Type, String ID, int Priority, int CSID, Intersection Intersection)
        {
            this.MsgCount++;
            float[] ts = new float[5];
            float[] te = new float[5];
            Message m;
            switch (Type)
            {
                case Definitions.MsgAuthorize:
                    m = new Message(Definitions.MsgAuthorize, Definitions.Agent, ID,
                            ts, te, -1.0f, Priority, CSID.ToString(), -1,"");//""站位

                    //写到消息队列
                    List<Message> AtoV_BufferTemp = Intersection.GetAtoV_Buffer();
                    AtoV_BufferTemp.Add(m);
                    Intersection.SetAtoV_Buffer(AtoV_BufferTemp);
                    MainData.UpdateIntersectionList(Intersection);                    
                    this.Token[CSID] = true;
                    //this.OutPutAuthMsg(1, m);
                    break;
                case Definitions.MsgRefuse:
                    break;
                case Definitions.MsgRaisePrio:
                    m = new Message(Definitions.MsgRaisePrio, Definitions.Agent, ID,
                            ts, te, -1.0f, Priority, CSID.ToString(), -1,"");
                    //写到消息队列
                    List<Message> AtoV_BufferTemp2 = Intersection.GetAtoV_Buffer();
                    AtoV_BufferTemp2.Add(m);
                    Intersection.SetAtoV_Buffer(AtoV_BufferTemp2);
                    MainData.UpdateIntersectionList(Intersection); 
                    //this.OutPutAuthMsg(3, m);
                    break;
                default:
                    System.Windows.Forms.MessageBox.Show("Wrong type of message from Agent ---- ", "Error", System.Windows.Forms.MessageBoxButtons.OK, System.Windows.Forms.MessageBoxIcon.Error);
                    return;
            }
        }

        /************************************************************************/
        /* 设置授权信息，TokenID为被授权的临界区号，ID是被授权车辆                                                                     */
        /************************************************************************/
        private void setToken(int TokenID, string ID)
        {
            int j = 0;
            int LaneID = FindLaneIDByVehicleID(ID);

            for (j = 0; j < this.LaneQueue[LaneID].Count; j++)
            {
                if (this.LaneQueue[LaneID].ElementAt(j).ID == ID)
                {
                    //LaneQueue中存储的信息

                    this.LaneQueue[LaneID].ElementAt(j).HaveTokenCount++;
                    this.LaneQueue[LaneID].ElementAt(j).Token[TokenID] = true;
                    this.LaneQueue[LaneID].ElementAt(j).VehicleManagers[TokenID].HasToken = true;
                    for (int i = 0; i < 5; i++)
                    {
                        if (this.LaneQueue[LaneID].ElementAt(j).VehicleManagers[i] != null)
                        {
                            this.LaneQueue[LaneID].ElementAt(j).VehicleManagers[i].Token[TokenID] = true;
                        }
                    }
                }
            }

            //CS中存储的信息
            foreach (KeyValuePair<string, VehicleManager> kvp in this.CS[TokenID])
            {
                if (kvp.Key == ID)
                {
                    kvp.Value.HasToken = true;
                    kvp.Value.Token[TokenID] = true;
                }
            }
            for (int i = 0; i < 5; ++i)
            {
                foreach (KeyValuePair<string, VehicleManager> kvp in this.CS[i])
                {
                    if (kvp.Key == ID)
                    {
                        kvp.Value.Token[TokenID] = true;
                    }
                }
            }

            //本文件中的车道标志信息
            this.Token[TokenID] = true;
            return;
        }

        /************************************************************************/
        /* 判断各临界区的申请队列是否为空则返回false，否则返回true                                                                     */
        /************************************************************************/
        private bool IsCSEmpty()
        {
            bool mark = true;
            for (int i = 1; i < 5; ++i)
            {
                if (this.CS[i].Count != 0)
                {
                    mark = false;
                }
            }
            return mark;
        }

        /************************************************************************/
        /* 将车辆是否选中、判断信息置位                                                                     */
        /************************************************************************/
        private void ReSetIsSelect()
        {
            for (int i = 0; i < 5; ++i)
            {
                for (int j = 0; j < this.CS[i].Count; ++j)
                {
                    this.CS[i].ElementAt(j).Value.isSelect = false;
                }
            }
            for (int i = 0; i < 5; ++i)
            {
                for (int j = 0; j < this.LaneQueue[i].Count; j++)
                {
                    this.LaneQueue[i].ElementAt(j).IsJudged = false;
                }
            }
            for (int i = 0; i < this.VehicleMng.Count; i++)
            {
                this.VehicleMng.ElementAt(i).Value.IsJudged = false;
            }
        }

        /************************************************************************/
        /* 判断当前车辆是否是当前车道的第一辆车，其中LaneID是车道名称，ID是需要判断的车辆
         * 若车辆ID是第一辆车，则返回ture，否则返回false*/
        /************************************************************************/
        private bool isFirstVehicle(int LaneID, string ID)
        {
            string IDtmp = null;
            for (int i = 0; i < this.LaneQueue[LaneID].Count; i++)
            {
                SuperVehicleManager stmp = this.LaneQueue[LaneID].ElementAt(i);
                string sections = stmp.Original_SectionID;
                int CSID = int.Parse(sections[0].ToString());
                if (stmp.Left[CSID] == true || stmp.Token[CSID] == true)
                {
                    continue;
                }
                else
                {
                    IDtmp = stmp.ID;
                    break;
                }
            }

            if (IDtmp == ID)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        /************************************************************************/
        /* computeDelayTime()中一旦出现非O所在车道的高优先级车辆V时，则返回到达时间在V之前的O所在车道中的车辆PV
         * 最后返回的值为PV在VehicleMng中的位置Index , 若PV不存在，则返回-1
         * 其中参数Index为V在VehicleMng中的位置信息        */
        /************************************************************************/
        private int GetPreVehiFromOriLane(int Index)
        {
            int pos = -1;
            int LaneID = 0;
            for (int i = 0; i < Index; i++)
            {
                string ID = this.VehicleMng.ElementAt(i).Key;
                Vehicle VehicleTemp = FindVehicleByID(ID);//通过ID找到Vehicle车辆，再通过车辆找到所在车道。即就是通过Vehicle类可以拿到车辆所在车道。
                String LaneIDTemp = VehicleTemp.Get_Current_LaneID();
                LaneID = int.Parse(LaneIDTemp.ElementAt(5).ToString());
                 
                if (this.HighPriorLaneIDOrigin.Contains(LaneID))
                {
                    pos = i;
                }
            }
            return pos;
        }

        /************************************************************************/
        /* 查找当前临界区CSID在section中的位置，返回其位置（从1开始计数）                                                                     */
        /************************************************************************/
        private int GetPosInSections(string sections, string CSID)
        {
            for (int i = 0; i < sections.Count(); i++)
            {
                if (sections[i].ToString() == CSID)
                {
                    return i + 1;
                }
            }
            return -1;
        }


        /************************************************************************/
        /* 计算VehicleMng中第index（从0开始计数）辆车的延迟时间,
         * Index与IndexPre之间的车辆可不计入延迟时间计算
         * 参数：  int Index  需计算的车辆在VehicelMng中的位置
         *         int CSID_Origin  原高优先级车辆的第一个临界区
         *         int Priority     高优先级
         *         ref float FirstVehcielTs  第一辆被计入车辆的到达时间
         * yg        
         */
        /************************************************************************/
        private float computeDelayTime(int Index, int CSID, int Priority)
        {
            Stack<int> Vehicle_count = new Stack<int>();

            int index_pre = 0;
            SuperVehicleManager stmp_pre = null;
            float ts_pre = 0, te_pre = 0;
            string ID_pre = "";

            int index_cur = 0;
            SuperVehicleManager stmp_cur = null;
            float ts_cur = 0, te_cur = 0;
            string ID_cur = "";

            //遍历所有的车辆
            for (int i = Index; i >= 0; i--)
            {
                SuperVehicleManager stmp = this.VehicleMng.ElementAt(i).Value;
                int pos = -1;
                //高优先级车辆
                if (stmp.Priority == Priority)
                {
                    //获取在VehicleMng中的，到达CSID时间在当前车辆之前的，是O所在车道车辆的时间距离当前车辆最近的车辆的pos
                    pos = this.GetPreVehiFromOriLane(i);
                    Vehicle_count.Push(i);

                    //高优先级车辆到O所在车道的前车之间的车辆不再计入延时计算
                    for (int j = i - 1; j > pos; j--)
                    {
                        stmp = this.VehicleMng.ElementAt(j).Value;
                        //若之间出现了其他车道的高优先级车辆时，需要将其加入Vehicle_count栈中
                        if (stmp.Priority == Priority)
                        {
                            Vehicle_count.Push(j);
                        }
                    }
                    //当O所在车道的前车不存在时，则break
                    if (pos == -1)
                    {
                        break;
                    }
                    //将O所在车道的，Index之前的车辆存入Vehicle_count栈中
                    //                     Vehicle_count.Push(pos);
                    //                     i = pos;
                    i = pos + 1;
                }
                else
                {
                    //若车辆V非O车道车辆，且V不是高优先级车辆，且V经过判断的无法被授权的车辆，可以从延迟计算中去除
                    String VehicleID = this.VehicleMng.ElementAt(i).Value.ID;
                    //Vehicle VehicleTemp = FindVehicleByID(ID);//通过ID找到Vehicle车辆，再通过车辆找到所在车道。即就是通过Vehicle类可以拿到车辆所在车道。
                    //String LaneIDTemp = VehicleTemp.Get_Current_LaneID();
                    //int LaneID = int.Parse(LaneIDTemp.ElementAt(5).ToString());

                    int LaneID = FindLaneIDByVehicleID(VehicleID);
                    if (this.HighPriorLaneIDOrigin.Contains(LaneID) == false)
                    {
                        //未经过判断、可获得部分临界区的车辆需要计入延时计算
                        if (this.VehicleMng.ElementAt(i).Value.IsAuthorized != 3)
                        {
                            Vehicle_count.Push(i);
                        }
                    }
                    else//车辆是O车道车辆
                    {
                        Vehicle_count.Push(i);
                    }
                }
            }

            //最终延迟时间
            float result = 0;
            if (Vehicle_count.Count == 1)
            {
                return 0;
            }

            while (Vehicle_count.Count > 1)
            {
                index_pre = Vehicle_count.Pop();
                stmp_pre = this.VehicleMng.ElementAt(index_pre).Value;
                ID_pre = stmp_pre.ID;
                this.GetCurrentTime(CSID, ID_pre, ref ts_pre, ref te_pre);

                index_cur = Vehicle_count.Peek();
                stmp_cur = this.VehicleMng.ElementAt(index_cur).Value;
                ID_cur = stmp_cur.ID;
                this.GetCurrentTime(CSID, ID_cur, ref ts_cur, ref te_cur);

                result += (te_pre - ts_cur);
            }
            return (float)Math.Round(result, Definitions.Precise);
        }


        /************************************************************************/
        /* xc VehicleMng是在ID之前的车辆排序
         * 
         * 生成全局队列Q+，用于PriorFIFO算法调用， 
         * 按照当前高优先级车辆的某一临界区进行排序,
         * 其中CSID为当前判断的临界区，deadline为当前判断的高优先级车辆在对应的CSID的到达时间
         * ProType标志函数的工作模式，若值为0，则用于生成VehicleMng，若值为1，则用于生成lst，以供computeLeftCSPreDelayTime计算使用
         * 最终返回当前优先车辆在VehicleMng的位置 */
        /************************************************************************/
        private List<string> CreateVehicleMng(int CSID, string ID, int ProType)
        {
            if (ProType == 0)
            {
                this.VehicleMng.Clear();
            }
            int EtoWIndex = 0, WtoEIndex = 0, NtoSIndex = 0, StoNIndex = 0;
            bool EtoWMark = false, WtoEMark = false, NtoSMark = false, StoNMark = false;
            float[] ts_tmp = new float[5];
            int pos = 0;
            string ID_tmp = null;
            SuperVehicleManager EtoWStmp = null, WtoEStmp = null, NtoSStmp = null, StoNStmp = null;
            string CSIDStr = CSID.ToString();
            List<string> lst = new List<string>();

            while (ID_tmp != ID)  //xc 循环条件不同，这个循环保证是在ID之前的车辆
            {
                for (int i = 0; i < 5; i++)
                {
                    ts_tmp[i] = 0;
                }
                EtoWMark = false;
                if (this.LaneQueue[Definitions.EtoW_Lane] != null)
                {
                    for (int i = EtoWIndex; i < this.LaneQueue[Definitions.EtoW_Lane].Count; i++)
                    {
                        if (this.LaneQueue[Definitions.EtoW_Lane].ElementAt(EtoWIndex).Original_SectionID.Contains(CSIDStr)
                            && this.LaneQueue[Definitions.EtoW_Lane].ElementAt(EtoWIndex).Token[CSID] == false
                            && this.LaneQueue[Definitions.EtoW_Lane].ElementAt(EtoWIndex).Left[CSID] == false)
                        {
                            EtoWStmp = this.LaneQueue[Definitions.EtoW_Lane].ElementAt(EtoWIndex);
                            EtoWMark = true;
                            pos = this.GetPosInSections(this.LaneQueue[Definitions.EtoW_Lane].ElementAt(EtoWIndex).Original_SectionID, CSIDStr);
                            ts_tmp[Definitions.EtoW_Lane] = this.LaneQueue[Definitions.EtoW_Lane].ElementAt(EtoWIndex).Ts[pos];
                            break;
                        }
                        else
                        {
                            EtoWIndex++;
                        }
                    }
                }
                if (!EtoWMark)
                {
                    ts_tmp[Definitions.EtoW_Lane] = Definitions.MaxTime + 1;
                }

                WtoEMark = false;
                if (this.LaneQueue[Definitions.WtoE_Lane] != null)
                {
                    for (int i = WtoEIndex; i < this.LaneQueue[Definitions.WtoE_Lane].Count; i++)
                    {
                        if (this.LaneQueue[Definitions.WtoE_Lane].ElementAt(WtoEIndex).Original_SectionID.Contains(CSIDStr)
                            && this.LaneQueue[Definitions.WtoE_Lane].ElementAt(WtoEIndex).Token[CSID] == false
                            && this.LaneQueue[Definitions.WtoE_Lane].ElementAt(WtoEIndex).Left[CSID] == false)
                        {
                            WtoEStmp = this.LaneQueue[Definitions.WtoE_Lane].ElementAt(WtoEIndex);
                            WtoEMark = true;
                            pos = this.GetPosInSections(this.LaneQueue[Definitions.WtoE_Lane].ElementAt(WtoEIndex).Original_SectionID, CSIDStr);
                            ts_tmp[Definitions.WtoE_Lane] = this.LaneQueue[Definitions.WtoE_Lane].ElementAt(WtoEIndex).Ts[pos];
                            break;
                        }
                        else
                        {
                            WtoEIndex++;
                        }
                    }
                }
                if (!WtoEMark)
                {
                    ts_tmp[Definitions.WtoE_Lane] = Definitions.MaxTime + 1;
                }

                NtoSMark = false;
                if (this.LaneQueue[Definitions.NtoS_Lane] != null)
                {
                    for (int i = NtoSIndex; i < this.LaneQueue[Definitions.NtoS_Lane].Count; i++)
                    {
                        if (this.LaneQueue[Definitions.NtoS_Lane].ElementAt(NtoSIndex).Original_SectionID.Contains(CSIDStr)
                            && this.LaneQueue[Definitions.NtoS_Lane].ElementAt(NtoSIndex).Token[CSID] == false
                            && this.LaneQueue[Definitions.NtoS_Lane].ElementAt(NtoSIndex).Left[CSID] == false)
                        {
                            NtoSStmp = this.LaneQueue[Definitions.NtoS_Lane].ElementAt(NtoSIndex);
                            NtoSMark = true;
                            pos = this.GetPosInSections(this.LaneQueue[Definitions.NtoS_Lane].ElementAt(NtoSIndex).Original_SectionID, CSIDStr);
                            ts_tmp[Definitions.NtoS_Lane] = this.LaneQueue[Definitions.NtoS_Lane].ElementAt(NtoSIndex).Ts[pos];
                            break;
                        }
                        else
                        {
                            NtoSIndex++;
                        }
                    }
                }
                if (!NtoSMark)
                {
                    ts_tmp[Definitions.NtoS_Lane] = Definitions.MaxTime + 1;
                }

                StoNMark = false;
                if (this.LaneQueue[Definitions.StoN_Lane] != null)
                {
                    for (int i = StoNIndex; i < this.LaneQueue[Definitions.StoN_Lane].Count; i++)
                    {
                        if (this.LaneQueue[Definitions.StoN_Lane].ElementAt(StoNIndex).Original_SectionID.Contains(CSIDStr)
                            && this.LaneQueue[Definitions.StoN_Lane].ElementAt(StoNIndex).Token[CSID] == false
                            && this.LaneQueue[Definitions.StoN_Lane].ElementAt(StoNIndex).Left[CSID] == false)
                        {
                            StoNStmp = this.LaneQueue[Definitions.StoN_Lane].ElementAt(StoNIndex);
                            StoNMark = true;
                            pos = this.GetPosInSections(this.LaneQueue[Definitions.StoN_Lane].ElementAt(StoNIndex).Original_SectionID, CSIDStr);
                            ts_tmp[Definitions.StoN_Lane] = this.LaneQueue[Definitions.StoN_Lane].ElementAt(StoNIndex).Ts[pos];
                            break;
                        }
                        else
                        {
                            StoNIndex++;
                        }
                    }
                }
                if (!StoNMark)
                {
                    ts_tmp[Definitions.StoN_Lane] = Definitions.MaxTime + 1;
                }

                float min = Definitions.MaxTime + 3;
                int laneID = 0;
                for (int i = Definitions.WtoE_Lane; i <= Definitions.StoN_Lane; i++)
                {
                    if ((min - ts_tmp[i]) > 0)
                    {
                        min = ts_tmp[i];
                        laneID = i;
                    }
                }

                if ((min - Definitions.MaxTime - 1) > -0.0001)
                {
                    Debug.WriteLine("fdsafds");
                    if (WtoEIndex == (this.LaneQueue[Definitions.WtoE_Lane].Count)
                        && EtoWIndex == (this.LaneQueue[Definitions.EtoW_Lane].Count)
                        && NtoSIndex == (this.LaneQueue[Definitions.NtoS_Lane].Count)
                        && StoNIndex == (this.LaneQueue[Definitions.StoN_Lane].Count))
                    {
                        break;
                    }
                    continue;
                }

                if (ProType == 0)
                {//用生成VehicleMng
                    switch (laneID)
                    {
                        case Definitions.WtoE_Lane:
                            WtoEIndex++;
                            this.VehicleMng.Add(WtoEStmp.ID, WtoEStmp);
                            ID_tmp = WtoEStmp.ID;
                            break;
                        case Definitions.EtoW_Lane:
                            EtoWIndex++;
                            this.VehicleMng.Add(EtoWStmp.ID, EtoWStmp);
                            ID_tmp = EtoWStmp.ID;
                            break;
                        case Definitions.NtoS_Lane:
                            NtoSIndex++;
                            this.VehicleMng.Add(NtoSStmp.ID, NtoSStmp);
                            ID_tmp = NtoSStmp.ID;
                            break;
                        case Definitions.StoN_Lane:
                            StoNIndex++;
                            this.VehicleMng.Add(StoNStmp.ID, StoNStmp);
                            ID_tmp = StoNStmp.ID;
                            break;
                    }
                }
                else if (ProType == 1)
                {//用于生成lst，以供computeLeftCSPreDelayTime计算使用                                        
                    //遍历所有的到达时间在i的CSID到达时间之前的车辆，创建lst
                    switch (laneID)
                    {
                        case Definitions.WtoE_Lane:
                            WtoEIndex++;
                            lst.Add(WtoEStmp.ID);
                            ID_tmp = WtoEStmp.ID;
                            break;
                        case Definitions.EtoW_Lane:
                            EtoWIndex++;
                            lst.Add(EtoWStmp.ID);
                            ID_tmp = EtoWStmp.ID;
                            break;
                        case Definitions.NtoS_Lane:
                            NtoSIndex++;
                            lst.Add(NtoSStmp.ID);
                            ID_tmp = NtoSStmp.ID;
                            break;
                        case Definitions.StoN_Lane:
                            StoNIndex++;
                            lst.Add(StoNStmp.ID);
                            ID_tmp = StoNStmp.ID;
                            break;
                    }
                }

                //没有车辆再可被授权
                if (WtoEIndex == (this.LaneQueue[Definitions.WtoE_Lane].Count)
                    && EtoWIndex == (this.LaneQueue[Definitions.EtoW_Lane].Count)
                    && NtoSIndex == (this.LaneQueue[Definitions.NtoS_Lane].Count)
                    && StoNIndex == (this.LaneQueue[Definitions.StoN_Lane].Count))
                {
                    break;
                }
            }
            return lst;
        }

        /************************************************************************/
        /* 返回当前高（中）优先级车辆下一个需要的临界区的到达时间                                                                     */
        /************************************************************************/
        private List<KeyValuePair<string, float>> GetHighPrioVehicleTs()
        {
            List<KeyValuePair<string, float>> tmp = new List<KeyValuePair<string, float>>();
            foreach (string ID in this.HighPriorVehicle)
            {
                SuperVehicleManager stmp = this.FindPosInLaneQueue(ID);
                string Sections = stmp.Original_SectionID;
                for (int i = 0; i < Sections.Count(); i++)
                {
                    int cs = int.Parse(Sections[i].ToString());
                    if (stmp.Token[cs] == false && stmp.Left[cs] == false)
                    {
                        KeyValuePair<string, float> kvp = new KeyValuePair<string, float>(stmp.ID, stmp.Ts[i + 1]);
                        tmp.Add(kvp);
                        break;
                    }
                }
            }
            return tmp;
        }

        /************************************************************************/
        /* 提升当前车辆的优先级，                                                                     */
        /************************************************************************/
        private void RaisePriority(string ID, int priority, Intersection Intersection)
        {
            if (this.HighPriorVehicle.Contains(ID) == false)
            {
                //在HighPriorVehicle中增加需要提升优先级的车辆
                this.HighPriorVehicle.Add(ID);
                //将需要提升优先级的车辆信息，发送至vehicle.cs中，便于最后的界面显示
                this.sendMessage(Definitions.MsgRaisePrio, ID, this.PriorType, 0, Intersection);

                Vehicle VehicleTemp = FindVehicleByID(ID);//通过ID找到Vehicle车辆，再通过车辆找到所在车道。即就是通过Vehicle类可以拿到车辆所在车道。
                String LaneIDTemp = VehicleTemp.Get_Current_LaneID();
                int LaneID = int.Parse(LaneIDTemp.ElementAt(5).ToString());
                for (int i = 0; i < this.LaneQueue[LaneID].Count; i++)
                {
                    if (this.LaneQueue[LaneID].ElementAt(i).ID == ID)
                    {
                        this.LaneQueue[LaneID].ElementAt(i).Priority = priority;
                    }
                }
            }
        }

        /************************************************************************/
        /* 用于判断是否还有临界区未被授权，即是否还有剩余的空临界区，若有剩余的空临界区，则返回true，否则返回false                                                                     */
        /************************************************************************/
        private bool ISCSUnauthorized()
        {
            for (int i = 0; i < 5; i++)
            {
                if (this.Token[i] == false)
                {
                    return true;
                }
            }
            return false;
        }

        /************************************************************************/
        /* 判断车辆是否占有着其他的临界区
         * yg*/
        /************************************************************************/
        private bool IsTokenCS(string ID)
        {
            SuperVehicleManager stmp = this.FindPosInLaneQueue(ID);
            for (int i = 1; i < 5; i++)
            {
                if (stmp.Token[i] == true)
                {
                    return true;
                }
            }
            return false;
        }

        /************************************************************************/
        /* 高优先级车辆的排序
         * 
         * 
         * 生成全局队列Q+，用于PriorFIFO算法调用， 
         * 按照当前高优先级车辆的某一临界区进行排序,
         * 其中CSID为当前判断的临界区，deadline为当前判断的高优先级车辆在对应的CSID的到达时间
         * ProType标志函数的工作模式，若值为0，则用于生成VehicleMng，若值为1，则用于生成lst，以供computeLeftCSPreDelayTime计算使用
         * 最终返回当前优先车辆在VehicleMng的位置 */
        /************************************************************************/
        private List<KeyValuePair<string, float>> HighPriorInLaneSorted()
        {
            int EtoWIndex = 0, WtoEIndex = 0, NtoSIndex = 0, StoNIndex = 0;
            bool EtoWMark = false, WtoEMark = false, NtoSMark = false, StoNMark = false;
            float[] ts_tmp = new float[5];
            int pos = 0;
            SuperVehicleManager EtoWStmp = null, WtoEStmp = null, NtoSStmp = null, StoNStmp = null;
            List<KeyValuePair<string, float>> lst = new List<KeyValuePair<string, float>>();
            int CSID = 0;
            string CSIDStr = "";

            while (true)
            {
                for (int i = 0; i < 5; i++)
                {
                    ts_tmp[i] = 0;
                }
                EtoWMark = false;
                if (this.LaneQueue[Definitions.EtoW_Lane] != null)
                {
                    CSID = 2;
                    CSIDStr = CSID.ToString();
                    for (int i = EtoWIndex; i < this.LaneQueue[Definitions.EtoW_Lane].Count; i++)
                    {
                        if (this.LaneQueue[Definitions.EtoW_Lane].ElementAt(EtoWIndex).Priority == this.PriorType
                            && this.LaneQueue[Definitions.EtoW_Lane].ElementAt(EtoWIndex).Token[CSID] == false
                            && this.LaneQueue[Definitions.EtoW_Lane].ElementAt(EtoWIndex).Left[CSID] == false)
                        {
                            EtoWStmp = this.LaneQueue[Definitions.EtoW_Lane].ElementAt(EtoWIndex);
                            EtoWMark = true;
                            pos = this.GetPosInSections(this.LaneQueue[Definitions.EtoW_Lane].ElementAt(EtoWIndex).Original_SectionID, CSIDStr);
                            ts_tmp[Definitions.EtoW_Lane] = this.LaneQueue[Definitions.EtoW_Lane].ElementAt(EtoWIndex).Ts[pos];
                            break; //找到高优先级、未占用2号临界区、未离开，记录到达2号临界区时间
                        }
                        else
                        {
                            EtoWIndex++;
                        }
                    }
                }
                if (!EtoWMark)
                {
                    ts_tmp[Definitions.EtoW_Lane] = Definitions.MaxTime + 1;
                }


                WtoEMark = false;
                if (this.LaneQueue[Definitions.WtoE_Lane] != null)
                {
                    CSID = 3;
                    CSIDStr = CSID.ToString();
                    for (int i = WtoEIndex; i < this.LaneQueue[Definitions.WtoE_Lane].Count; i++)
                    {
                        if (this.LaneQueue[Definitions.WtoE_Lane].ElementAt(WtoEIndex).Priority == this.PriorType
                            && this.LaneQueue[Definitions.WtoE_Lane].ElementAt(WtoEIndex).Token[CSID] == false
                            && this.LaneQueue[Definitions.WtoE_Lane].ElementAt(WtoEIndex).Left[CSID] == false)
                        {
                            WtoEStmp = this.LaneQueue[Definitions.WtoE_Lane].ElementAt(WtoEIndex);
                            WtoEMark = true;
                            pos = this.GetPosInSections(this.LaneQueue[Definitions.WtoE_Lane].ElementAt(WtoEIndex).Original_SectionID, CSIDStr);
                            ts_tmp[Definitions.WtoE_Lane] = this.LaneQueue[Definitions.WtoE_Lane].ElementAt(WtoEIndex).Ts[pos];
                            break;
                        }
                        else
                        {
                            WtoEIndex++;
                        }
                    }
                }
                if (!WtoEMark)
                {
                    ts_tmp[Definitions.WtoE_Lane] = Definitions.MaxTime + 1;
                }

                StoNMark = false;
                if (this.LaneQueue[Definitions.StoN_Lane] != null)
                {
                    CSID = 4;
                    CSIDStr = CSID.ToString();
                    for (int i = StoNIndex; i < this.LaneQueue[Definitions.StoN_Lane].Count; i++)
                    {
                        if (this.LaneQueue[Definitions.StoN_Lane].ElementAt(StoNIndex).Priority == this.PriorType
                            && this.LaneQueue[Definitions.StoN_Lane].ElementAt(StoNIndex).Token[CSID] == false
                            && this.LaneQueue[Definitions.StoN_Lane].ElementAt(StoNIndex).Left[CSID] == false)
                        {
                            StoNStmp = this.LaneQueue[Definitions.StoN_Lane].ElementAt(StoNIndex);
                            StoNMark = true;
                            pos = this.GetPosInSections(this.LaneQueue[Definitions.StoN_Lane].ElementAt(StoNIndex).Original_SectionID, CSIDStr);
                            ts_tmp[Definitions.StoN_Lane] = this.LaneQueue[Definitions.StoN_Lane].ElementAt(StoNIndex).Ts[pos];
                            break;
                        }
                        else
                        {
                            StoNIndex++;
                        }
                    }
                }
                if (!StoNMark)
                {
                    ts_tmp[Definitions.StoN_Lane] = Definitions.MaxTime + 1;
                }

                NtoSMark = false;
                if (this.LaneQueue[Definitions.NtoS_Lane] != null)
                {
                    CSID = 1;
                    CSIDStr = CSID.ToString();
                    for (int i = NtoSIndex; i < this.LaneQueue[Definitions.NtoS_Lane].Count; i++)
                    {
                        if (this.LaneQueue[Definitions.NtoS_Lane].ElementAt(NtoSIndex).Priority == this.PriorType
                            && this.LaneQueue[Definitions.NtoS_Lane].ElementAt(NtoSIndex).Token[CSID] == false
                            && this.LaneQueue[Definitions.NtoS_Lane].ElementAt(NtoSIndex).Left[CSID] == false)
                        {
                            NtoSStmp = this.LaneQueue[Definitions.NtoS_Lane].ElementAt(NtoSIndex);
                            NtoSMark = true;
                            pos = this.GetPosInSections(this.LaneQueue[Definitions.NtoS_Lane].ElementAt(NtoSIndex).Original_SectionID, CSIDStr);
                            ts_tmp[Definitions.NtoS_Lane] = this.LaneQueue[Definitions.NtoS_Lane].ElementAt(NtoSIndex).Ts[pos];
                            break;
                        }
                        else
                        {
                            NtoSIndex++;
                        }
                    }
                }
                if (!NtoSMark)
                {
                    ts_tmp[Definitions.NtoS_Lane] = Definitions.MaxTime + 1;
                }

                //排序，找到最小的
                float min = Definitions.MaxTime + 3;
                int laneID = 0;
                for (int i = Definitions.WtoE_Lane; i <= Definitions.StoN_Lane; i++)
                {
                    if ((min - ts_tmp[i]) > 0)
                    {
                        min = ts_tmp[i];
                        laneID = i;
                    }
                }

                //一般不执行，除非没车了
                if ((min - Definitions.MaxTime - 1) > -0.0001)
                {
                    if (WtoEIndex == (this.LaneQueue[Definitions.WtoE_Lane].Count)
                        && EtoWIndex == (this.LaneQueue[Definitions.EtoW_Lane].Count)
                        && NtoSIndex == (this.LaneQueue[Definitions.NtoS_Lane].Count)
                        && StoNIndex == (this.LaneQueue[Definitions.StoN_Lane].Count))
                    {
                        break;
                    }
                    continue;
                }

                KeyValuePair<string, float> kvp;
                switch (laneID)
                {
                    case Definitions.WtoE_Lane:
                        WtoEIndex++;
                        kvp = new KeyValuePair<string, float>(WtoEStmp.ID, ts_tmp[Definitions.WtoE_Lane]);
                        lst.Add(kvp);
                        break;
                    case Definitions.EtoW_Lane:
                        EtoWIndex++;
                        kvp = new KeyValuePair<string, float>(EtoWStmp.ID, ts_tmp[Definitions.EtoW_Lane]);
                        lst.Add(kvp);
                        break;
                    case Definitions.NtoS_Lane:
                        NtoSIndex++;
                        kvp = new KeyValuePair<string, float>(NtoSStmp.ID, ts_tmp[Definitions.NtoS_Lane]);
                        lst.Add(kvp);
                        break;
                    case Definitions.StoN_Lane:
                        StoNIndex++;
                        kvp = new KeyValuePair<string, float>(StoNStmp.ID, ts_tmp[Definitions.StoN_Lane]);
                        lst.Add(kvp);
                        break;
                }

                //没有车辆再可被授权
                if (WtoEIndex == (this.LaneQueue[Definitions.WtoE_Lane].Count)
                    && EtoWIndex == (this.LaneQueue[Definitions.EtoW_Lane].Count)
                    && NtoSIndex == (this.LaneQueue[Definitions.NtoS_Lane].Count)
                    && StoNIndex == (this.LaneQueue[Definitions.StoN_Lane].Count))
                {
                    break;
                }
            }
            return lst;
        }


        /************************************************************************/
        /* 按照到达下个临界区的时间先后顺序，返回高优先级车辆的列表
         * Priority是当前查找的临界区，
         */
        /************************************************************************/
        private void SortPrioVehicle(int Priority)
        {
            List<KeyValuePair<string, float>> tmp = this.GetHighPrioVehicleTs();
            List<KeyValuePair<string, float>> tmp1 = new List<KeyValuePair<string, float>>();   //存储当前占用临界区的高优先级车辆
            //List<KeyValuePair<string, float>> tmp2 = new List<KeyValuePair<string, float>>();   //存储当前未占用临界区的高优先级车辆
            List<KeyValuePair<string, float>> tmp3;

            //按照车辆到达临界区的时间顺序进行排序
            for (int i = 0; i < tmp.Count; i++)
            {
                string ID = tmp.ElementAt(i).Key;
                KeyValuePair<string, float> kvp = tmp.ElementAt(i);
                int index = 0;
                int j = 0;
                if (this.IsTokenCS(ID) == true) //这辆ID车还占用其他的临界区
                {
                    for (j = 0; j < tmp1.Count; j++)
                    {
                        if ((tmp1.ElementAt(j).Value - kvp.Value) > 0)
                        {
                            index = j;
                            break;
                        }
                    }
                    tmp1.Insert(j, kvp);
                }
            }

            tmp3 = this.HighPriorInLaneSorted();  //未占用临界区的车辆排序Sort


            this.HighPriorVehiclesSorted.Clear();
            //将占用临界区的车辆存入HighPriorVehiclesSorted中
            foreach (KeyValuePair<string, float> kvp in tmp1)
            {
                string ID = kvp.Key;
                SuperVehicleManager stmp = this.FindPosInLaneQueue(ID);
                this.HighPriorVehiclesSorted.Add(stmp);
            }

            //将未占用临界区的车辆加入HighPriorVehiclesSorted
            foreach (KeyValuePair<string, float> kvp in tmp3)
            {
                string ID = kvp.Key;
                SuperVehicleManager stmp = this.FindPosInLaneQueue(ID);  //将ID转换成SuperVehicleManger
                this.HighPriorVehiclesSorted.Add(stmp);
            }
        }

        /************************************************************************/
        /*xc 找到ID对应的SuperVehicleManger 并返回SuperVehicleManger */
        /* 找到当前ID对应的LaneQueue中的位置，最终返回两个位置posx,posy，其中posx表示在第车道号，其中posy表示在在车道队列中的位置                                                                     */
        /************************************************************************/
        private SuperVehicleManager FindPosInLaneQueue(string ID)
        {

            Vehicle VehicleTemp = FindVehicleByID(ID);//通过ID找到Vehicle车辆，再通过车辆找到所在车道。即就是通过Vehicle类可以拿到车辆所在车道。
            String LaneIDTemp = VehicleTemp.Get_Current_LaneID();            

            SuperVehicleManager stmp = null;
            //int i = int.Parse(LaneIDTemp.ElementAt(5).ToString());

            for (int i = 1; i <= 4; i++) 
            {
                for (int j = 0; j < this.LaneQueue[i].Count; j++)
                {
                    if (this.LaneQueue[i].ElementAt(j).ID == ID)
                    {
                        return this.LaneQueue[i].ElementAt(j);
                    }
                }
            }               
            return stmp;
        }

        private SuperVehicleManager FindPosInCSLaneQueue(string ID)
        {

            //Vehicle VehicleTemp = FindVehicleByID(ID);//通过ID找到Vehicle车辆，再通过车辆找到所在车道。即就是通过Vehicle类可以拿到车辆所在车道。
            //String LaneIDTemp = VehicleTemp.Get_Previous_LaneID();

            SuperVehicleManager stmp = null;
            //int i = int.Parse(LaneIDTemp.ElementAt(5).ToString());

            //int i = FindLaneIDByVehicleID(ID)

            for (int i = 1; i <= 4; i++) 
            {
                for (int j = 0; j < this.LaneQueue[i].Count; j++)
                {
                    if (this.LaneQueue[i].ElementAt(j).ID == ID)
                    {
                        return this.LaneQueue[i].ElementAt(j);
                    }
                }
            }                
            return stmp;
        }

        /************************************************************************/
        /* 查找车俩当前需判断的下一个临界区,最终返回当前车辆的CSID                                                                    */
        /************************************************************************/
        private int GetNextCSID(string ID)
        {
            int CSID = 0;
            SuperVehicleManager stmp = this.FindPosInLaneQueue(ID);
            string Sections = stmp.Original_SectionID;
            for (int i = 0; i < Sections.Count(); i++)
            {
                CSID = int.Parse(Sections[i].ToString());
                if (stmp.Left[CSID] == false && stmp.Token[CSID] == false)
                {
                    return CSID;
                }
            }
            return 0;
        }

        /************************************************************************/
        /* 找到当前ID对应的CS中的位置，最终返回两个位置posx,posy，其中posx表示在第车道号，其中posy表示在在车道队列中的位置                                                                     */
        /************************************************************************/
        private VehicleManager FindPosInCS(string ID)
        {
            VehicleManager vtmp = null;
            int i = FindLaneIDByVehicleID(ID);
            //int i = int.Parse(ID[0].ToString());
            for (int j = 0; j < this.CS[i].Count; j++)
            {
                if (this.CS[i].ElementAt(j).Value.getID() == ID)
                {
                    return this.CS[i].ElementAt(j).Value;
                }
            }
            return vtmp;
        }

        /************************************************************************/
        /* 检测是否有高优先级车辆出现在预约队列中,若有高优先级车辆出现，则返回true,否则返回false                                                                     */
        /************************************************************************/
        private bool isPriorVehiOccur()
        {
            bool mark = false;
            for (int i = 0; i < 5; i++)
            {
                for (int j = 0; j < this.LaneQueue[i].Count(); j++)
                {
                    if (this.LaneQueue[i].ElementAt(j).Priority == Definitions.Prio_High)
                    {
                        mark = true;
                        this.PriorType = Definitions.Prio_High;
                        return true;
                    }
                    else if (this.LaneQueue[i].ElementAt(j).Priority == Definitions.Prio_Mid)
                    {
                        mark = true;
                        this.PriorType = Definitions.Prio_Mid;
                    }
                }
            }
            if (mark == true)
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        /************************************************************************/
        /* 获取当前的原高优先级车辆和原高优先级车辆车道                                                                     */
        /************************************************************************/
        private void OriginalHighPriorVehicleQueueCreate(int priority)
        {
            //HighPriorVehicleOrigin 原高优先级车辆，其中float为到达下一个临界区的时间，但是尽量不做排序
            //HighPriorLaneIDOrigin 当前原高优先级车辆所处的车道
            this.HighPriorVehicleOrigin.Clear();
            this.HighPriorLaneIDOrigin.Clear();

            for (int i = 0; i < 5; i++)
            {
                for (int j = 0; j < this.LaneQueue[i].Count; j++)
                {
                    //选择优先级为priority优先级的车辆
                    if (this.LaneQueue[i].ElementAt(j).Original_Priority == priority)
                    {
                        //加入HighPriorVehicleOrigin队列
                        if (this.HighPriorVehicleOrigin.Contains(this.LaneQueue[i].ElementAt(j).ID) == false)
                        {
                            this.HighPriorVehicleOrigin.Add(this.LaneQueue[i].ElementAt(j).ID);
                        }

                        //拿出每一个需要的临界区
                        string sections = this.LaneQueue[i].ElementAt(j).Original_SectionID;
                        bool mark = true;
                        for (int k = 0; k < sections.Count(); k++)
                        {
                            int CSID = int.Parse(sections[k].ToString());
                            if (this.LaneQueue[i].ElementAt(j).Left[CSID] == true || this.LaneQueue[i].ElementAt(j).Token[CSID] == true)
                            {
                                mark = false;
                            }
                        }
                        //将优先级车道加入HighPriorLaneIDOrigin
                        if (mark && this.HighPriorLaneIDOrigin.Contains(i) == false)
                        {
                            this.HighPriorLaneIDOrigin.Add(i);
                        }
                    }
                }
            }
        }

        /************************************************************************/
        /* 提升占有当前高优先级车辆所需临界区的车辆                                                                     */
        /************************************************************************/
        private void HighPriorVehicleQueueCreate(int priority,Intersection Intersection)
        {
            //因为当高优先级车辆到达时，若之前时间片是处理中优先级车辆，那么此时需要更新原来的HighPriorVehicle
            //对原先HighPriorVehicle中是高优先级的车辆保留，否则删除
            //当低优先级车辆到来时无需如此
            if (this.PriorType == Definitions.Prio_High)
            {
                List<string> High_Tmp = new List<string>();
                for (int i = 0; i < this.HighPriorVehicle.Count; i++)
                {
                    string ID = this.HighPriorVehicle.ElementAt(i);
                    SuperVehicleManager stmp = this.FindPosInCSLaneQueue(ID);

                    //xc
                    try
                    {
                        if (stmp.Priority == Definitions.Prio_High)
                        {
                            High_Tmp.Add(ID);
                        }
                    }
                    catch (Exception e)
                    {
                        System.Console.WriteLine(e.GetType());
                    }
                   
                }
                this.HighPriorVehicle.Clear();
                this.HighPriorVehicle = High_Tmp;
            }

            //将所有的高优先级车辆存储到HighPriorVehicle中
            for (int i = 0; i < this.HighPriorVehicleOrigin.Count; i++)
            {
                string ID = this.HighPriorVehicleOrigin.ElementAt(i);
                //检测HighPriorVehicle是否有当前需要键入的高优先级车辆，若为true则表示当前存在该车辆，无需再加入
                if (this.HighPriorVehicle.Contains(ID) == false)
                {
                    this.HighPriorVehicle.Add(this.HighPriorVehicleOrigin.ElementAt(i));
                }
            }

            //当前占用临界区的车辆需要提升优先级
            for (int i = 0; i < this.Authorize_CS_to_Vehicle.Count; i++)
            {
                string ID = this.Authorize_CS_to_Vehicle.ElementAt(i).Value;
                SuperVehicleManager stmp = this.FindPosInCSLaneQueue(ID);

                //xc
                try
                {
                    if (stmp.Priority < this.PriorType)
                    {
                        this.RaisePriority(ID, priority, Intersection);
                    }
                }
                catch (Exception e)
                {
                    System.Console.WriteLine(e.GetType());
                }

                
            }

            int num = 0;
            //当HighPriorVehicle中没有新元素加入时，结束循环
            while (this.HighPriorVehicle.Count != num)
            {
                num = this.HighPriorVehicle.Count;
                for (int i = 0; i < this.HighPriorVehicle.Count; i++)
                {
                    string ID = this.HighPriorVehicle.ElementAt(i);
                    SuperVehicleManager stmp = this.FindPosInLaneQueue(ID);

                    string Sections = stmp.Original_SectionID;
                    for (int j = 0; j < Sections.Count(); j++)
                    {
                        int cs = int.Parse(Sections[j].ToString());
                        //若所需临界区被占用，则将占用其临界区的车辆加入到HighPriorVehicle中
                        if (this.Token[cs] == true)
                        {
                            ID = this.Authorize_CS_to_Vehicle[cs];  //xc占用临界区的车辆ID
                            //提升车辆的优先级，并将其加入到HighPriorVehicle中
                            this.RaisePriority(ID, priority, Intersection);
                        }
                    }
                }
            }

        }

        //根据车辆ID拿到其所在车道的ID
        public int FindLaneIDByVehicleID(string ID)
        {
            for (int i = 1; i <= 4; i++)
            {
                for (int j = 0; j < this.LaneQueue[i].Count; j++)
                {
                    if (this.LaneQueue[i].ElementAt(j).ID == ID)
                    {
                        return i;
                    }
                }
            }
            return -1;
        }

        /************************************************************************/
        /* 判断当前车辆V所在车道，到达时间在V之前，且未被授权的车辆，若有未被授权的车辆,则返回true，反之，返回false                                                                     */
        /************************************************************************/
        private bool ISPreVehiNotAuth(string ID)
        {
            Vehicle VehicleTemp = FindVehicleByID(ID);//通过ID找到Vehicle车辆，再通过车辆找到所在车道。即就是通过Vehicle类可以拿到车辆所在车道。
            String LaneIDTemp = VehicleTemp.Get_Current_LaneID();
            int LaneID = int.Parse(LaneIDTemp.ElementAt(5).ToString());
            
            //此处遍历CS，而非遍历VehicleMng中的内容，因为每次当前判断的高优先级临界区的一条VehicelMng被创建，
            for (int i = 0; i < this.LaneQueue[LaneID].Count; i++)
            {
                string IDtmp = this.LaneQueue[LaneID].ElementAt(i).ID;
                SuperVehicleManager stmp = this.FindPosInLaneQueue(IDtmp);
                int CSID = int.Parse(stmp.Original_SectionID[0].ToString());
                if (stmp.Token[CSID] == true || stmp.Left[CSID] == true)
                {
                    continue;
                }
                if (stmp.IsAuthorized == 3 && stmp.IsJudged == true)   //xc 则表示stmp已经过判断，未被授权临界区
                {
                    //车辆未被授权
                    return true;
                }
                if (IDtmp == ID)
                {
                    break;
                }
            }
            return false;
        }

        /************************************************************************/
        /* 获取ID车辆在VehicleMng中的位置
         * ID为所需查找车辆的ID,最终返回其在Index中的位置，从0开始计数
         */
        /************************************************************************/
        private int GetPosInVehicleMng(string ID)
        {
            for (int i = 0; i < this.VehicleMng.Count; i++)
            {
                if (ID == this.VehicleMng.ElementAt(i).Key)
                {
                    return i;
                }
            }
            return -1;
        }

        /************************************************************************/
        /* 获取当前车辆的前车在VehicleMng中的位置信息,从0开始计数                                                                     */
        /************************************************************************/
        private int getPreVehicleIndex(int LaneID, string ID)
        {
            List<SuperVehicleManager> vq = this.LaneQueue[LaneID];
            string s = null;

            for (int i = 0; i < this.LaneQueue[LaneID].Count; i++)
            {
                if (vq.ElementAt(i).ID == ID)
                {
                    s = vq.ElementAt(i - 1).ID;
                    break;
                }
            }
            for (int i = 0; i < this.VehicleMng.Count; i++)
            {
                if (this.VehicleMng.ElementAt(i).Value.ID == s)
                {
                    return i;
                }
            }
            return -1;
        }

        /************************************************************************/
        /* 设定车辆能够被存入CanBeAuthorized中                                                                     */
        /************************************************************************/
        private void StoreInCanBeAuthorized(int CSID, string ID)
        {
            if (this.CanBeAuthorized[CSID].Contains(ID) == false)
            {
                this.CanBeAuthorized[CSID].Add(ID);
            }
        }

        /************************************************************************/
        /* 将车辆从CanBeAuthorized中删除，存入到CanNotBeAuthorized中                                                                     */
        /************************************************************************/
        private void RemoveFromCanBeAuthorized(int CSID, string ID)
        {
            if (this.CanBeAuthorized[CSID].Contains(ID) == true)
            {
                this.CanBeAuthorized[CSID].Remove(ID);
            }
        }

        /************************************************************************/
        /* 判断某一低优先级车辆是否造成高优先级车辆与O所在车道车辆的延迟                                                                     */
        /************************************************************************/
        private void IsDelayPriorVehicle(int index, int CSID)
        {
            List<SuperVehicleManager> lst = new List<SuperVehicleManager>();

            //加入前面的需要可以被授权的车辆
            for (int i = 0; i < index; i++)
            {
                SuperVehicleManager stmp = this.VehicleMng.ElementAt(i).Value;
                string ID = stmp.ID;
                if (this.CanBeAuthorized[CSID].Contains(ID))
                {
                    lst.Add(stmp);
                }
            }
            //加入当前车
            lst.Add(this.VehicleMng.ElementAt(index).Value);
            //加入之后的高优先级高优先级车辆，O所在车道
            for (int i = index + 1; i < this.VehicleMng.Count; i++)
            {
                SuperVehicleManager stmp = this.VehicleMng.ElementAt(i).Value;
                string ID = stmp.ID;
                int LaneID = stmp.LaneID;
                if (stmp.Priority == this.PriorType || this.HighPriorLaneIDOrigin.Contains(LaneID))
                {
                    if (this.CanBeAuthorized[CSID].Contains(ID))
                    {
                        lst.Add(stmp);
                    }
                }
            }

            //获取VehicleMng中index元素，在lst中的位置
            int Indexlst = 0;
            for (int i = 0; i < lst.Count; i++)
            {
                if (this.VehicleMng.ElementAt(index).Value.ID == lst.ElementAt(i).ID)
                {
                    Indexlst = i;
                    break;
                }
            }
            //计算VehicleMng中index元素，在lst中所造成的延迟
            float DelayTime = 0;
            if (Indexlst == 0)
            {//lst中Indexlst之前没有元素，延迟时间为0
                DelayTime = 0;
            }
            else
            {//lst中Indexlst之前有元素，需计算累计延迟时间
                for (int i = 1; i <= Indexlst; i++)
                {
                    SuperVehicleManager stmp_pre = lst.ElementAt(i - 1);
                    string ID_pre = stmp_pre.ID;
                    float ts_pre = 0, te_pre = 0;
                    this.GetCurrentTime(CSID, ID_pre, ref ts_pre, ref te_pre);

                    SuperVehicleManager stmp_cur = lst.ElementAt(i);
                    string ID_cur = stmp_cur.ID;
                    float ts_cur = 0, te_cur = 0;
                    this.GetCurrentTime(CSID, ID_cur, ref ts_cur, ref te_cur);

                    DelayTime += te_pre - ts_cur;
                }
            }
            if (lst.Count - 1 > Indexlst)
            {//lst中Indexlst之后有可被授权的高优先级车辆H，此时需要检验是否会造成H的延迟
                SuperVehicleManager stmp_pre = lst.ElementAt(Indexlst);
                string ID_pre = stmp_pre.ID;
                float ts_pre = 0, te_pre = 0;
                this.GetCurrentTime(CSID, ID_pre, ref ts_pre, ref te_pre);

                SuperVehicleManager stmp_cur = lst.ElementAt(Indexlst + 1);
                string ID_cur = stmp_cur.ID;
                float ts_cur = 0, te_cur = 0;
                this.GetCurrentTime(CSID, ID_cur, ref ts_cur, ref te_cur);

                if ((DelayTime + te_pre - ts_cur) < 0.000000001)
                {//不造成延迟
                    this.CanBeAuthorized[CSID].Add(lst.ElementAt(Indexlst).ID);
                }
            }
            else
            {//lst中Indexlst之后没有可被授权的高优先级车辆H
                this.CanBeAuthorized[CSID].Add(lst.ElementAt(Indexlst).ID);
            }
        }

        /************************************************************************/
        /* 处理当前需判断的高优先级车辆之前的车辆
         * 参数：High_LaneID  高优先级车辆车道
         *       High_Index   高优先级车辆在VehicleMng中的位置
         *       mark     标志
         */
        /************************************************************************/
        private void ProcPreVehiInFirstCS(int High_LaneID, int High_CSID, int High_Index, int mark)
        {
            SuperVehicleManager High_stmp = this.VehicleMng.ElementAt(High_Index).Value;
            string High_ID = High_stmp.ID;
            float High_Ts = 0, High_Te = 0;
            this.GetCurrentTime(High_CSID, High_ID, ref High_Ts, ref High_Te);

            for (int i = 0; i < High_Index; i++)
            {
                SuperVehicleManager stmp = this.VehicleMng.ElementAt(i).Value;
                string ID = stmp.ID;
                int laneID = stmp.LaneID;
                //高优先级车辆
                if (stmp.Priority == this.PriorType && stmp.IsJudged == true)
                {
                    continue;
                }
                //O所在车道低优先级车辆
                if (this.HighPriorLaneIDOrigin.Contains(laneID) == true)
                {
                    if (stmp.IsJudged == false && this.isFirstVehicleInLane(laneID, ID, High_CSID))
                    {
                        this.StoreInCanBeAuthorized(High_CSID, ID);
                    }
                }
                else
                {//非O所在车道低优先级车辆
                    int index = this.GetPosInVehicleMng(ID);
                    if (stmp.IsJudged == false && this.isFirstVehicleInLane(laneID, ID, High_CSID))
                    {
                        this.IsDelayPriorVehicle(index, High_CSID);
                    }
                }
            }
            this.StoreInCanBeAuthorized(High_CSID, High_ID);

        }

        /************************************************************************/
        /* 处理当前的高优先级车辆H的第一个临界区，获得O车辆到达时间的先后顺序进行排序的车辆序列，并返回不会被延迟（未被选中的第一辆高优先级车辆）。 
         * 参数：   SuperVehicleManager Original_stmp   原高优先级车辆
         *          SuperVehicleManager High_stmp       当前处理的高优先级车辆
         * 结果：  返回第一辆能够被存入CanBeAuth中的车辆在VehicleMng中的位置信息index中,从0开始计数
         *         若其值为-1，那么说明此时需要判断的高优先级车辆信息中，没有能够被授权或者提升的高优先级车辆 
         *         
         * xc
         * High_stmp：临时变量的SuperVehicleManger
         * Priority：优先级别
         * High_Index：临时变量在VehicleMng中的排序
         * 
         * //xcxc High_stmp是HighPriorVehiclesSorted中临时变量、index_tmp是High_stmp在VehicleMng中的位置
         * 
         */
        /************************************************************************/
        private int ProcFirstCS(SuperVehicleManager High_stmp, int Priority, int High_Index)
        {
            string High_ID = High_stmp.ID;              //当前进行判断的高优先级车辆的ID
            string High_Sections = High_stmp.Original_SectionID;        //当前所判断的高优先级车辆的临界区
            int High_CSID = this.GetNextCSID(High_ID);  //获取当前第一辆车所到达的下一个临界区,此时的High_CSID未被占用
            int High_LaneID = High_stmp.LaneID;         //当前高优先级车辆的原车道

            float DelayTime = 0;        //实际计算所得的延迟时间
            Stack<int> stk = new Stack<int>();  //存储当前需要提升的高优先级车辆
            bool mark = this.ISPreVehiNotAuth(High_ID);

            //车辆V之前有V所在车道车辆未被授权，即必然会导致当前车辆V的延迟，则将其放入到CanNotBeAuthorized中。
            //当前所判断的高优先级车辆之前有车辆未被授权，此时会导致高优先级车辆的延迟，
            //所以在High_Index之前，到其前一个高优先级车辆之间的车辆，都可以被认为无法被授权、
            //若车辆已被授权，则无需再管
            //若车辆未被授权，那么此时则将其全部从CanBeAuth中删除（此处可以不做任何处理，因为CanBeAuth中本身应该没哟值）
            if (mark == true)
            {
                this.RemoveFromCanBeAuthorized(High_CSID, High_ID);
                return -1;
            }

            //xc 能进入这个函数的，High_CSID一定没有被占用，在之前的函数有判断，如果被占用，是不能被counte
            if (this.Token[High_CSID] == true)
            {//此时的High_CSID未被占用
                System.Windows.Forms.MessageBox.Show("ProcFirstCS error1");
            }

            //此时的高优先级车辆，必然有需要的临界区，且总共分为两类：
            /* 其中之一为：原高优先级车道的有前车，未占用临界区的高优先级车辆，这些车辆存在前车可以继承优先级的情况，需要对前车进行优先级即判断
             * 另一是：占有临界区的高优先级车辆，这些车辆没有前车
             */
            if (this.IsTokenCS(High_ID) == false)
            {
                //未占用临界区
                //车辆V之前没有V所在车道车辆未被授权，即此时需要找到当前可以被提升的第一辆车
                int index = this.GetPosInVehicleMng(High_ID);
                DelayTime = this.computeDelayTime(index, High_CSID, Priority);


                if ((DelayTime > 0) && (this.isFirstVehicle(High_LaneID, High_ID) == true))
                {//如果高优先级车辆被延迟，且车辆是首车                    
                    this.ProcPreVehiInFirstCS(High_LaneID, High_CSID, index, 0);
                    return -1;
                }
                //若车辆是首车，或者是其他车道的车辆，那么就不会出现找到前车的情况，即循环最终不会满足
                SuperVehicleManager stmp = this.VehicleMng.ElementAt(index).Value;
                while ((DelayTime > 0) && (this.isFirstVehicle(stmp.LaneID, stmp.ID) == false))
                {
                    if (stk.Contains(index) == false)
                    {
                        stk.Push(index);
                    }
                    //当前车辆出现延迟，不是首车,说明会被延迟，此时从CanBeAuth中删除
                    this.RemoveFromCanBeAuthorized(High_CSID, stmp.ID);
                    index = this.getPreVehicleIndex(stmp.LaneID, stmp.ID);
                    stmp = this.VehicleMng.ElementAt(index).Value;
                    DelayTime = this.computeDelayTime(index, High_CSID, Priority);
                }
                if (DelayTime <= 0)
                {//循环结束时，若不出现延时，则存入CanBeAuth中
                    this.StoreInCanBeAuthorized(High_CSID, stmp.ID);
                    this.ProcPreVehiInFirstCS(High_LaneID, High_CSID, index, 1);
                    return index;
                }
                else
                {
                    //循环结束时，出现延时，则说明index车辆已经指向到Original_Land的首车，
                    //那么此时说明车辆无法被授权，需要将需要返回-1
                    this.ProcPreVehiInFirstCS(High_LaneID, High_CSID, index, 0);
                    return -1;
                }
            }
            else
            {
                //占用临界区，没有前需的临界区
                //当前判断车辆不是O车道的高优先级车辆，
                //其中包括Original_CSID被占用，但是车辆无需占用Original_CSID的情况
                int index = this.GetPosInVehicleMng(High_ID);
                if (this.Token[High_CSID] == true)
                {//非O车道车辆，若其下一个所需临界区已被授权，那么此时需要将其置入CanNotBeAuth中
                    this.RemoveFromCanBeAuthorized(High_CSID, High_ID);
                }
                else
                {//若其下一个临界区未被授权，则判断其是否会造成延迟,若不会造成延迟，而其下一个临界区未被授权，则可存入到CanBeAuthorized中
                    DelayTime = this.computeDelayTime(index, High_CSID, Priority);
                    if (DelayTime <= 0)
                    {//不出现延时
                        this.StoreInCanBeAuthorized(High_CSID, High_ID);
                        this.ProcPreVehiInFirstCS(High_LaneID, High_CSID, index, 2);
                    }
                    else
                    {//出现延迟
                        this.RemoveFromCanBeAuthorized(High_CSID, High_ID);
                        this.ProcPreVehiInFirstCS(High_LaneID, High_CSID, index, 3);
                    }
                }
            }
            return -1;
        }

        /************************************************************************/
        /* 判断车辆是否有前需临界区，并返回最后一个前需临界区的在其所需临界区中的位置index,参数start表示第一个开始的临界区，若无前需临界区，那么就返回-1                                                                     */
        /************************************************************************/
        private int HavePreNeedCS(int index, int High_CSID, ref int start)
        {
            SuperVehicleManager stmp = this.VehicleMng.ElementAt(index).Value;
            string Sections = stmp.Original_SectionID;
            int CSID = 0;
            int mark = -1;
            start = 0;
            for (int i = 0; i < Sections.Count(); i++)
            {
                CSID = int.Parse(Sections[i].ToString());
                if (CSID == High_CSID)
                {
                    break;
                }
                if (stmp.Token[CSID] == false && stmp.Left[CSID] == false)
                {
                    mark = i;
                }
                else
                {
                    start = i + 1;
                }
            }
            return mark;
        }

        /************************************************************************/
        /* 判断车辆是否有后续临界区，并返回第一个后需临界区的在其所需临界区中的位置index，若无后续临界区，返回-1                                                                       */
        /************************************************************************/
        private int HaveSubNeedCS(int index, int High_CSID)
        {
            SuperVehicleManager stmp = this.VehicleMng.ElementAt(index).Value;
            string Sections = stmp.Original_SectionID;
            int CSID = 0;
            int mark = 0;
            for (int i = 0; i < Sections.Count(); i++)
            {
                CSID = int.Parse(Sections[i].ToString());
                if (CSID == High_CSID)
                {
                    mark = i;
                    break;
                }
            }

            if (Sections.Count() > mark)
            {
                for (int i = (mark + 1); i < Sections.Count(); i++)
                {
                    CSID = int.Parse(Sections[i].ToString());
                    if (stmp.Token[CSID] == false && stmp.Left[CSID] == false)
                    {
                        return i;
                    }
                    else
                    {
                        System.Windows.Forms.MessageBox.Show("HaveSubNeedCS error");
                    }
                }
            }
            return -1;
        }

        /************************************************************************/
        /* 按照当前车辆是否已经被判断授权，来返回其到达、离开某一临界区的时间                                                                     */
        /************************************************************************/
        private void GetCurrentTime(int CSID, string ID, ref float ts, ref float te)
        {
            SuperVehicleManager stmp = this.FindPosInLaneQueue(ID);
            string section = stmp.Original_SectionID;
            int index = this.GetPosInSections(section, CSID.ToString());

            if (stmp.IsAuthorized == 0 || stmp.IsJudged == false)
            {
                ts = stmp.Ts[index];
                te = stmp.Te[index];
            }
            else
            {
                if (stmp.RealTsToken[index] == true)
                {
                    ts = stmp.Ts[index];
                }
                else
                {
                    ts = Definitions.MaxTime;
                }

                if (stmp.RealTeToken[index] == true)
                {
                    te = stmp.Te[index];
                }
                else
                {
                    te = Definitions.MaxTime;
                }
            }
        }

        /************************************************************************/
        /* 判断车辆P是否是当前车辆V的前车,前车ID为pre,后车为ID，所判断的车道为LaneID                                                                    */
        /************************************************************************/
        private bool ISFrontVehicleInLane(string pre, string ID, int LaneID)
        {
            bool mark = false;
            for (int i = 0; i < this.LaneQueue[LaneID].Count; i++)
            {
                if (this.LaneQueue[LaneID].ElementAt(i).ID == ID)
                {
                    break;
                }
                if (pre == this.LaneQueue[LaneID].ElementAt(i).ID)
                {
                    mark = true;
                }
            }
            return mark;
        }

        /************************************************************************/
        /* 计算延迟时间lst中的延迟时间，
         * lst是当前用于计算延迟时间的列表，Origianal_LaneID为高优先级车辆所在车道，LaneID是当前判断车辆所在的车道,CSID是当前进行判断的临界区
         * 最终返回车辆被延迟的时间，若其值大于0，则说明不延迟，若其值小于0，则说明延迟
         */
        /************************************************************************/
        private float computeLeftCSPreDelayTime(List<string> lst, int LaneID, int CSID)
        {
            List<int> Vehicle_count = new List<int>();  //存储当前需判断的两个高优先级车辆之间的车辆 + 之前需保留的正在判断车道的车辆
            List<int> Vehicle_Range = new List<int>();  //存储当前需判断的两个高优先级车辆之间的车辆
            List<int> Vehicle_left = new List<int>();   //
            List<int> High = new List<int>();           //存储当前高优先级车辆的在lst中的位置
            List<int> Vehicle_sorted = new List<int>(); //

            High.Add(-1);
            //获取lst中的高优先级车辆，O所在车道车辆
            for (int i = 0; i < lst.Count; i++)
            {
                string ID_tmp = lst[i];
                SuperVehicleManager stmp = this.FindPosInLaneQueue(ID_tmp);
                if (stmp.Priority == this.PriorType || this.HighPriorLaneIDOrigin.Contains(stmp.LaneID))
                {
                    High.Add(i);
                }
            }

            Vehicle_left = null;
            //遍历所有的高优先级车辆
            for (int i = 1; i < High.Count; i++)
            {
                Vehicle_count.Clear();
                Vehicle_count = Vehicle_left;

                Vehicle_Range.Clear();
                for (int j = High[i - 1] + 1; j <= High[i]; j++)
                {
                    Vehicle_Range.Add(j);
                }
                if (Vehicle_count == null)
                {
                    Vehicle_count = Vehicle_Range;
                }
                else
                {
                    Vehicle_count.AddRange(Vehicle_Range);
                }

                //当前高优先级车辆的信息
                string ID_High = lst[High[i]];
                SuperVehicleManager s_High = this.FindPosInLaneQueue(ID_High);
                float Ts_High = 0, Te_High = 0;
                this.GetCurrentTime(CSID, ID_High, ref Ts_High, ref Te_High);

                //遍历此时的所有的车辆信息
                for (int j = 0; j < Vehicle_count.Count; j++)
                {
                    string ID = lst[Vehicle_count[j]];
                    SuperVehicleManager s = this.FindPosInLaneQueue(ID);
                    float Ts = 0, Te = 0;
                    this.GetCurrentTime(CSID, ID, ref Ts, ref Te);
                    //当前车辆不是高优先级车辆H，或者不是原高优先级车辆所在车道车辆OL，可以被H和OL车辆所延迟（舍弃）
                    if (s.Priority != this.PriorType && this.HighPriorLaneIDOrigin.Contains(s.LaneID) == false)
                    {
                        //车辆会造成H和OL的延迟，可以舍弃
                        if ((Te - Ts_High) > 0)
                        {
                            //当前车辆是当前判断车辆所在车道的前车，需要在最后的计算中
                            if (s.LaneID == LaneID)
                            {
                                Vehicle_left.Add(Vehicle_count[j]);
                            }
                        }
                        else//不会造成延迟
                        {
                            Vehicle_sorted.Add(Vehicle_count[j]);
                        }
                    }
                    else
                    {//此时应当不会出现这种情况，因为每次遍历的是两个相邻的高优先级车辆、O所在车道车辆
                        Vehicle_sorted.Add(Vehicle_count[j]);
                    }
                }
            }
            //遍历最后一个高优先级车辆之前的当前判断车道之前的车辆
            if (Vehicle_left != null)
            {
                for (int j = 0; j < Vehicle_left.Count; j++)
                {
                    Vehicle_sorted.Add(Vehicle_left[j]);
                }
            }

            //遍历最后一个高优先级车辆之后的剩余的车辆
            for (int j = High[High.Count - 1] + 1; j < lst.Count; j++)
            {
                Vehicle_sorted.Add(j);
            }

            //最终延迟时间
            float result = 0;
            if (Vehicle_sorted != null)
            {
                if (Vehicle_sorted.Count() > 0)
                {
                    for (int i = 1; i < Vehicle_sorted.Count; i++)
                    {
                        string ID_pre = lst[Vehicle_sorted[i - 1]];
                        float ts_pre = 0, te_pre = 0;
                        this.GetCurrentTime(CSID, ID_pre, ref ts_pre, ref te_pre);

                        string ID_cur = lst[Vehicle_sorted[i]];
                        float ts_cur = 0, te_cur = 0;
                        this.GetCurrentTime(CSID, ID_cur, ref ts_cur, ref te_cur);

                        result += (te_pre - ts_cur);
                    }
                }
            }
            return (float)Math.Round(result, Definitions.Precise);
        }

        /************************************************************************/
        /* ProcDelayInLeftCSs                                                                     */
        /************************************************************************/
        private void ProcDelayVehicleInLeftCSs(SuperVehicleManager stmp, int CSID_tmp)
        {
            //若当前车辆V是高优先级车辆或者O车道车辆，V未经过判断，V为车道首车，则可被授权CSID
            //以防止其他造成车辆V延迟的高优先级车辆P、O车道车辆P，在后续的判断中无法授权，从而导致的V再次可被授权，而引起的V授权错误
            //而若P车辆可被授权，则此时临界区已经被授出，V也无法获得临界区
            string ID = stmp.ID;
            Vehicle VehicleTemp = FindVehicleByID(ID);//通过ID找到Vehicle车辆，再通过车辆找到所在车道。即就是通过Vehicle类可以拿到车辆所在车道。
            String LaneIDTemp = VehicleTemp.Get_Current_LaneID();
            int LaneID = int.Parse(LaneIDTemp.ElementAt(5).ToString());

            if (this.HighPriorLaneIDOrigin.Contains(LaneID) || stmp.Priority == this.PriorType)
            {
                if (stmp.IsJudged == false)//车辆未被判断
                {
                    if (this.isFirstVehicleInLane(LaneID, ID, CSID_tmp) || this.IsTokenCS(stmp.ID))
                    {
                        this.StoreInCanBeAuthorized(CSID_tmp, ID);
                    }
                    else//车辆既没有占用临界区，也不是首车，即无论前车能否被授权，都无法被授权
                    {
                        this.RemoveFromCanBeAuthorized(CSID_tmp, ID);
                    }
                }
            }
            else
            {
                this.RemoveFromCanBeAuthorized(CSID_tmp, ID);
            }
        }

        /************************************************************************/
        /* 处理其余车辆的剩余临界区，判断其会否被延迟
         * 参数:  preindex    前一次进行判断的车辆的序号
         *        index       当前进行判断的高优先级车辆
         *        High_CSID   高优先级车辆的第一个临界区
         *        Origianl_LaneID   原高优先级车辆的车道
         */
        /************************************************************************/
        private void ProcLeftCSs(int Index, int High_CSID)
        {
            bool mark = false;
            //上一次判断高优先级车辆，遍历当前高优先级车辆之间的 所有车辆
            for (int i = 0; i <= Index; i++)
            {
                mark = false;
                SuperVehicleManager stmp = this.VehicleMng.ElementAt(i).Value;
                string ID = stmp.ID;
                string Sections = stmp.Original_SectionID;
                int LaneID = stmp.LaneID;

                if (this.CanBeAuthorized[High_CSID].Contains(ID) == false)
                {//若出现此种情况，则无需再判断其他临界区的CanBeAuthorized的值，无需再将车辆ID加到其中的CanBeAuthorized中
                    continue;
                }
                /************************************************************************/
                /* 处理前需临界区                                                                     */
                /************************************************************************/
                int start = 0;
                int PreNeedIndex = this.HavePreNeedCS(i, High_CSID, ref start);
                //判断当前所判断车辆i的每一个前需临界区CSID，创建申请时间在i之前的lst，判断当前的lst中的车辆是否会导致i的延迟
                for (int j = start; j <= PreNeedIndex; j++)
                {
                    int CSID_tmp = int.Parse(stmp.Original_SectionID[j].ToString());
                    if (this.Token[CSID_tmp] == true)
                    {
                        //若存在前需临界区无法被授权的情况，则此时无需再去判断剩余的临界区的情况，其后面的临界区必然不能被授权,所以此时判断VehicleMng中的剩余的车辆
                        this.RemoveFromCanBeAuthorized(CSID_tmp, ID);
                        mark = true;
                        break;
                    }

                    List<string> lst = new List<string>();
                    //遍历所有的到达时间在i的CSID到达时间之前的车辆，创建lst
                    lst = this.CreateVehicleMng(CSID_tmp, ID, 1);
                    float result = this.computeLeftCSPreDelayTime(lst, LaneID, CSID_tmp);

                    if (result <= 0)
                    {//不被延迟
                        this.StoreInCanBeAuthorized(CSID_tmp, ID);
                    }
                    else
                    {//被延迟
                        //若当前车辆V是高优先级车辆或者O车道车辆，V未经过判断，V为车道首车，则可被授权CSID
                        //以防止其他造成车辆V延迟的高优先级车辆P、O车道车辆P，在后续的判断中无法授权，从而导致的V再次可被授权，而引起的V授权错误
                        //而若P车辆可被授权，则此时临界区已经被授出，V也无法获得临界区
                        this.ProcDelayVehicleInLeftCSs(stmp, CSID_tmp);
                    }
                }

                if (mark == true)
                {
                    continue;
                }

                /************************************************************************/
                /* 处理后需临界区                                                                     */
                /************************************************************************/
                float DelayTime = 0;
                int SubNeedIndex = this.HaveSubNeedCS(i, High_CSID);
                if (SubNeedIndex == -1)
                {
                    continue;
                }
                for (int j = SubNeedIndex; j < Sections.Count(); j++)
                {
                    int CSID_tmp = int.Parse(Sections[j].ToString());
                    if (this.Token[CSID_tmp] == true)
                    {
                        this.RemoveFromCanBeAuthorized(CSID_tmp, ID);
                        break;
                    }
                    List<string> lst = new List<string>();
                    //遍历所有的到达时间在i的CSID到达时间之前的车辆，创建lst
                    lst = this.CreateVehicleMng(CSID_tmp, ID, 1);

                    DelayTime = this.computeLeftCSPreDelayTime(lst, LaneID, CSID_tmp);
                    if (DelayTime > 0)//车辆被延迟
                    {
                        //若当前车辆V是高优先级车辆或者O车道车辆，V未经过判断，V为车道首车，则可被授权CSID
                        //以防止其他造成车辆V延迟的高优先级车辆P、O车道车辆P，在后续的判断中无法授权，从而导致的V再次可被授权，而引起的V授权错误
                        //而若P车辆可被授权，则此时临界区已经被授出，V也无法获得临界区
                        this.ProcDelayVehicleInLeftCSs(stmp, CSID_tmp);
                    }
                    else
                    {
                        this.StoreInCanBeAuthorized(CSID_tmp, ID);
                    }
                }
            }
        }

        /************************************************************************/
        /* 判断高优先级车辆算法中，一辆车是否是按照临界区先后循序进行申请的，返回true则非法                                                                     */
        /************************************************************************/
        private bool isPriorTokenSequenceIllegal(int index, int LaneID, string ID, int AuthCSID)
        {
            SuperVehicleManager stmp = this.VehicleMng.ElementAt(index).Value;
            int d = 0;
            string sections = stmp.Original_SectionID;

            if (stmp != null)
            {
                //获取当前需要判断的临界区，即将要处理的临界区
                for (int i = 0; i < stmp.CS_Count; i++)
                {
                    if (int.Parse(stmp.Original_SectionID[i].ToString()) == AuthCSID)
                    {
                        d = i;
                        break;
                    }
                }

                //检测之前的临界区是否满足条件
                for (int i = 0; i < d; i++)
                {
                    int CSID = int.Parse(stmp.Original_SectionID[i].ToString());
                    if (stmp.Token[CSID] == false)
                    {//前车辆未被授权
                        return true;
                    }
                }
            }
            return false;
        }

        /************************************************************************/
        /* priority表示当前所判断车辆的当前的优先级
         * 判断当前车辆是否能够被授权，哪几个临界区能够被授权
         * 此模块中需要判断车辆是否延时，是否能够通过死锁、乱序、首车的判断
         * 返回值标志是否能够被授权，若其值为false，则表示不能被授权，若其值为true,则能被授权
         * 最终返回的值为最终按照先后顺序，能够被授权的临界区的第一个临界区StartNum，最后一个临界区EndNum（从0开始计数），
         * 如果出现一个临界区都无法被授权的情况，那么就将StartNum、EndNum
         * 返回值： 1：  不是首车或者乱序
         *          2：  出现死锁
         *          3：  车辆出现延迟
         *          4：  CSID可以被授权给车辆
         */
        /************************************************************************/
        private int JudgeCanBeAuth(int Index, int CSID)
        {
            string ID = this.VehicleMng.ElementAt(Index).Key;
            SuperVehicleManager stmp = this.FindPosInLaneQueue(ID);
            string sections = stmp.Original_SectionID;
            int LaneID = stmp.LaneID;

            //检测首车、乱序
            if (this.isPriorTokenSequenceIllegal(Index, LaneID, ID, CSID) == false
                && (this.isFirstVehicleInLane(LaneID, ID, CSID) == true))
            {
                if (this.Authorize_CS_to_Vehicle.ContainsKey(CSID))
                {
                    Debug.WriteLine("Authorize_CS_to_Vehicle error");
                }
            }
            else
            {
                return 1;
            }
            //检测死锁
            this.Authorize_CS_to_Vehicle.Add(6, ID);
            if (this.isTokenSequenceDeadLock(this.Authorize_CS_to_Vehicle) != true)//不会死锁
            {
                this.Authorize_CS_to_Vehicle.Remove(6);
            }
            else//会产生死锁
            {
                this.Authorize_CS_to_Vehicle.Remove(6);
                return 2;
            }
            //检测是否会延迟
            if (this.CanBeAuthorized[CSID].Contains(ID) == false)
            {
                return 3;
            }
            else
            {//车辆不会延迟
                return 4;
            }
        }

        /************************************************************************/
        /* 授权车辆以及其临界区
         * 需要处理的参数内容：
         * MultiSCSPrioFIFO_Sche：
         *      LaneQueue中的SuperVehicleManager信息、CS中的VehicleManager信息、Authorize_CS_to_Vehicle、Authorized_Vehicle_Sequence、
         *      Already_Authorized_All_CS_Vehicles、Token、CanBeAuthorized  
         * SuperVehicleManager：     
         *      VehicleManagers、Priority、HaveTokenCount、Token、Left、IsAuthorized 、RealTs、RealTe
         * VehicleManager：
         *      CurrentPriority、Token、HasToken、isSelect？？？
         */
        /************************************************************************/
        private void SetPrioToken(string ID, int StartNum, int EndNum, string sections, Intersection Intersection)
        {
            if (StartNum < EndNum)
            {
                //更新Authorized_Vehicle_Sequence中的信息
                if (this.Authorized_Vehicle_Sequence.Contains(ID) == false)
                {
                    this.Authorized_Vehicle_Sequence.Add(ID);
                }

                //更新Already_Authorized_All_CS_Vehicles中的信息
                if (sections.Count() == EndNum && this.Already_Authorized_All_CS_Vehicles.Contains(ID) == false)
                {
                    this.Already_Authorized_All_CS_Vehicles.Add(ID);
                }

                //将当前的所有临界区进行授权
                for (int i = StartNum; i < EndNum; i++)
                {
                    int CSID = int.Parse(sections[i].ToString());
                    //更新Authorize_CS_to_Vehicle中的信息
                    if (this.Authorize_CS_to_Vehicle.Keys.Contains(CSID) == false)
                    {
                        this.Authorize_CS_to_Vehicle.Add(CSID, ID);
                    }
                    //token
                    this.Token[CSID] = true;
                    this.sendMessage(Definitions.MsgAuthorize, ID, this.PriorType, CSID,Intersection);

                    bool mark = true;
                    Vehicle VehicleTemp = FindVehicleByID(ID);//通过ID找到Vehicle车辆，再通过车辆找到所在车道。即就是通过Vehicle类可以拿到车辆所在车道。
                    String LaneIDTemp = VehicleTemp.Get_Current_LaneID();
                    int LaneID = int.Parse(LaneIDTemp.ElementAt(5).ToString());
                    
                    for (int j = 0; j < this.LaneQueue[LaneID].Count; j++)
                    {
                        if (this.LaneQueue[LaneID].ElementAt(j).ID == ID)
                        {
                            this.LaneQueue[LaneID].ElementAt(j).Priority = this.PriorType;

                            this.LaneQueue[LaneID].ElementAt(j).Token[CSID] = true;
                            this.LaneQueue[LaneID].ElementAt(j).HaveTokenCount++;
                            this.LaneQueue[LaneID].ElementAt(j).VehicleManagers[CSID].HasToken = true;
                            this.LaneQueue[LaneID].ElementAt(j).RealTsToken[i + 1] = true;
                            this.LaneQueue[LaneID].ElementAt(j).RealTeToken[i + 1] = true;

                            if (EndNum == sections.Count())
                            {
                                this.LaneQueue[LaneID].ElementAt(j).IsAuthorized = 1;
                            }
                            else if (EndNum < sections.Count() && EndNum != 0)
                            {
                                this.LaneQueue[LaneID].ElementAt(j).IsAuthorized = 2;
                            }
                            else
                            {
                                this.LaneQueue[LaneID].ElementAt(j).IsAuthorized = 3;
                            }
                            mark = false;
                        }
                    }

                    if (mark)
                    {
                        Debug.WriteLine("fdsafsafd");
                    }
                }

                //CS中存储的信息
                for (int i = StartNum; i < EndNum; i++)
                {
                    int CSID = int.Parse(sections[i].ToString());
                    foreach (KeyValuePair<string, VehicleManager> kvp in this.CS[CSID])
                    {
                        if (kvp.Key == ID)
                        {
                            kvp.Value.HasToken = true;
                            kvp.Value.Token[CSID] = true;
                            kvp.Value.setCurrentPriority(this.PriorType);
                        }
                    }
                    for (int j = 0; j < 5; j++)
                    {
                        foreach (KeyValuePair<string, VehicleManager> kvp in this.CS[j])
                        {
                            if (kvp.Key == ID)
                            {
                                kvp.Value.Token[CSID] = true;
                            }
                        }
                    }
                }
            }
        }

        /************************************************************************/
        /* executeAlgorithmPrior算法中，按照到达时间先后的顺序，遍历当前的VehicleMng中的车辆，进行授权授权。
         * 需更新当前的HighPriorVehicle、IsPriorQueueChanged的信息，RealTs、RealTe
         * 参数：  Origianl_LaneID     原高优先级车辆所在车道
         * 返回值： 最终返回此时可以被授权的车辆列表
         */
        /************************************************************************/
        private List<string> AuthorizePriorVehicles(Intersection Intersection)
        {
            List<string> lst = new List<string>();
            for (int i = 0; i < this.VehicleMng.Count; i++)
            {
                //所有临界区已被授权完毕
                if (this.ISCSUnauthorized() == false)
                {

                    this.SetAuthorizedInfor(this.VehicleMng.ElementAt(i).Key);
                    break;
                }
                string ID = this.VehicleMng.ElementAt(i).Key;
                SuperVehicleManager stmp = this.VehicleMng.ElementAt(i).Value;
                string sections = stmp.Original_SectionID;
                int priority = stmp.Priority;

                Vehicle VehicleTemp = FindVehicleByID(ID);//通过ID找到Vehicle车辆，再通过车辆找到所在车道。即就是通过Vehicle类可以拿到车辆所在车道。
                String LaneIDTemp = VehicleTemp.Get_Current_LaneID();
                int LaneID = int.Parse(LaneIDTemp.ElementAt(5).ToString());
                
                int mark = -1;
                bool ContinueMark = true;   //标志车辆此时出现死锁、乱序、延时的情况
                bool StartMark = true;      //标志车辆到达第一个所需的临界区
                bool TokenMark = false;     //标志车辆因为所需临界区被占用而导致break
                int StartNum = 0, EndNum = sections.Count();

                //车辆已经经历过授权判断
                if (stmp.IsJudged == true)
                {
                    continue;
                }

                //判断每一个临界区
                for (int j = 0; j < sections.Count(); j++)
                {
                    if (!ContinueMark)
                    {

                        break;
                    }
                    int CSID = int.Parse(sections[j].ToString());
                    //如果临界区不被需要，那么继续此时的循环
                    if (stmp.Token[CSID] == true || stmp.Left[CSID] == true)
                    {

                        continue;
                    }


                    if (StartMark == true)
                    {

                        StartNum = j;
                        StartMark = false;
                    }


                    //当前临界区已被授权，此时可以结束后面临界区的判断
                    if (this.Token[CSID] == true)
                    {
                        EndNum = j;
                        TokenMark = true;
                        mark = 3;
                        break;
                    }
                    mark = this.JudgeCanBeAuth(i, CSID);
                    if (mark == 1 || mark == 2 || mark == 3)
                    {
                        //不是首车或者乱序
                        //出现死锁
                        //车辆出现延迟
                        EndNum = j;
                        ContinueMark = false;
                        break;
                    }
                    else if (mark == 4)
                    {
                        //车辆不出现延迟，CSID可以被授权给车辆
                        this.VehicleMng.ElementAt(i).Value.Token[CSID] = true;
                    }
                }
                if (TokenMark == true)
                {
                    for (int j = StartNum; j < EndNum; j++)
                    {
                        int CSID = int.Parse(sections[j].ToString());
                        if (this.Authorize_CS_to_Vehicle.Keys.Contains(CSID))
                        {
                            if (this.Authorize_CS_to_Vehicle[CSID] == ID)
                            {
                                Debug.WriteLine("sdaf");
                            }
                            else
                            {
                                this.VehicleMng.ElementAt(i).Value.Token[CSID] = false;
                            }
                        }
                        else
                        {
                            this.VehicleMng.ElementAt(i).Value.Token[CSID] = false;
                        }
                    }
                }

                if (EndNum == sections.Count())
                {//所有的临界区都满足条件
                    this.SetPrioToken(ID, StartNum, EndNum, sections, Intersection);
                    if (StartNum < EndNum)
                    {
                        lst.Add(ID);
                    }
                }
                else
                {//有车辆不满足条件
                    if (mark == 1 || mark == 2)
                    {
                        //不是首车或者乱序，此时之前对VehicleMng授权需重置,此时车辆不能授权
                        //出现死锁，此时之前对VehicleMng授权需重置,此时车辆不能授权
                        for (int j = StartNum; j < EndNum; j++)
                        {

                            int CSID = int.Parse(sections[j].ToString());
                            if (this.Authorize_CS_to_Vehicle.Keys.Contains(CSID))
                            {
                                if (this.Authorize_CS_to_Vehicle[CSID] == ID)
                                {
                                    Debug.WriteLine("sdaf");
                                }
                                else
                                {

                                    this.VehicleMng.ElementAt(i).Value.Token[CSID] = false;
                                }
                            }
                            else
                            {
                                this.VehicleMng.ElementAt(i).Value.Token[CSID] = false;
                            }
                        }
                    }
                    else if (mark == 3)
                    {//车辆出现延迟
                        //此时车辆若是高优先级车辆，或者O车道车辆，则可授权
                        if (stmp.Priority == this.PriorType || this.HighPriorLaneIDOrigin.Contains(LaneID))
                        {

                            this.SetPrioToken(ID, StartNum, EndNum, sections, Intersection);
                            if (StartNum < EndNum)
                            {
                                lst.Add(ID);
                            }
                        }
                        else
                        {//若车辆不是高优先级车辆，或者O车道车辆，那么此时的车辆不能被授权

                            for (int j = StartNum; j < EndNum; j++)
                            {
                                int CSID = int.Parse(sections[j].ToString());
                                if (this.Authorize_CS_to_Vehicle.Keys.Contains(CSID))
                                {
                                    if (this.Authorize_CS_to_Vehicle[CSID] == ID)
                                    {
                                        Debug.WriteLine("sdaf");
                                    }
                                    else
                                    {
                                        this.VehicleMng.ElementAt(i).Value.Token[CSID] = false;
                                    }
                                }
                                else
                                {
                                    this.VehicleMng.ElementAt(i).Value.Token[CSID] = false;
                                }
                            }
                        }
                    }
                }
                this.SetAuthorizedInfor(this.VehicleMng.ElementAt(i).Key);
            }
            return lst;
        }

        /************************************************************************/
        /* 判断车辆是否已经获得所有的临界区                                                                     */
        /************************************************************************/
        private bool IsAllCSToken(string ID)
        {
            SuperVehicleManager stmp = this.FindPosInLaneQueue(ID);
            for (int i = 0; i < stmp.Original_SectionID.Count(); i++)
            {
                int CSID = int.Parse(stmp.Original_SectionID[i].ToString());
                if (stmp.Token[CSID] == false && stmp.Left[CSID] == false)
                {
                    return false;
                }
            }
            return true;
        }

        /************************************************************************/
        /* 按照当前车辆占有的临界区，设置车辆的IsAuthorized、IsJudged信息 
         * yg*/
        /************************************************************************/
        private void SetAuthorizedInfor(string ID)
        {
            SuperVehicleManager stmp = this.FindPosInLaneQueue(ID);
            string sections = stmp.Original_SectionID;

            //xc 将LaneQueue中的SuperVehicleManger对应的IsJudged、IsAuthorized设置好
            Vehicle VehicleTemp = FindVehicleByID(ID);//通过ID找到Vehicle车辆，再通过车辆找到所在车道。即就是通过Vehicle类可以拿到车辆所在车道。
            String LaneIDTemp = VehicleTemp.Get_Current_LaneID();
            int LaneID = int.Parse(LaneIDTemp.ElementAt(5).ToString());
            
            for (int i = 0; i < this.LaneQueue[LaneID].Count; i++)
            {
                if (this.LaneQueue[LaneID].ElementAt(i).ID == ID)
                {
                    if (this.IsTokenCS(ID) == false)
                    {
                        this.LaneQueue[LaneID].ElementAt(i).IsAuthorized = 3;
                    }
                    else if (this.IsAllCSToken(ID))
                    {
                        this.LaneQueue[LaneID].ElementAt(i).IsAuthorized = 1;
                    }
                    else
                    {
                        this.LaneQueue[LaneID].ElementAt(i).IsAuthorized = 2;
                    }
                    this.LaneQueue[LaneID].ElementAt(i).IsJudged = true;
                    break;
                }
            }

            //xc 将VehicleMng中的SuperVehicleManger对应的IsJudged、IsAuthorized设置好
            int pos = this.GetPosInVehicleMng(ID);  //ID车辆在VehicleMng中的位置
            if (pos != -1)
            {
                if (this.IsTokenCS(ID) == false)
                {
                    this.VehicleMng[ID].IsAuthorized = 3;
                }
                else if (this.IsAllCSToken(ID))
                {
                    this.VehicleMng[ID].IsAuthorized = 1;
                }
                else
                {
                    this.VehicleMng[ID].IsAuthorized = 2;
                }
                this.VehicleMng[ID].IsJudged = true;
            }
        }

        /************************************************************************/
        /* 处理高优先级车辆                                                                     */
        /************************************************************************/
        private void processPriority(int Priority, Intersection Intersection)
        {
            bool IsPriorQueueChanged = true;        //标志高优先级车辆队列是否进行了更新，若为true，则表示已经进行了更新
            string High_Sections = null;            //当前所判断的高优先级车辆的临界区
            int High_CSID = 0;                      //当前高优先级车辆的当前正在判断的临界区
            int High_Index = 0;                     //当前到正在判断的高优先级车辆
            SuperVehicleManager High_stmp = null;   //当前进行判断的高优先级车辆
            string High_ID = null;                  //当前进行判断的高优先级车辆的ID
            int High_LaneID = 0;
            List<string> NewAddHighVehiclesLst = new List<string>();

            if (this.HighPriorVehicleOrigin != null)
            {
                IsPriorQueueChanged = true;
                //循环结束条件：没有新的高优先级车辆加入HighPriorVehicles中
                while (IsPriorQueueChanged)
                {
                    //更新当前的HighPriorVehicle中车辆
                    this.HighPriorVehicleQueueCreate(Priority, Intersection);
                    //按照已占用临界区的车辆 + 到达下一个临界区时间最短  的顺序进行排序
                    this.SortPrioVehicle(Priority);

                    IsPriorQueueChanged = false;
                    High_Index = 0;


                    //HighPriorVehiclesSorted中存储着占用或者没有占用临界区的高优先级车辆
                    //遍历所有的高优先级车辆
                    //结束条件2，全部高优先级车辆遍历完
                    for (int j = 0; j < this.HighPriorVehiclesSorted.Count; j++)
                    {
                        High_stmp = this.HighPriorVehiclesSorted.ElementAt(j);
                        High_ID = High_stmp.ID;

                        //获取Vehicle，拿LaneID
                        Vehicle VehicleTemp = FindVehicleByID(High_ID);
                        String LaneIDTemp = VehicleTemp.Get_Current_LaneID();
                        High_LaneID = int.Parse(LaneIDTemp.ElementAt(5).ToString());

                        High_CSID = this.GetNextCSID(High_ID);  //获取当前第一辆车所到达的下一个临界区
                        if (High_CSID == 0)
                        {
                            continue;
                        }
                        High_Sections = High_stmp.Original_SectionID;


                        //当前循环结束条件：当前所需判断的临界区已被授权
                        if (this.Token[High_CSID])
                        {
                            this.SetAuthorizedInfor(High_ID); //按照当前车辆占有的临界区，设置车辆（SuperVehicleManger）的IsAuthorized、IsJudged信息 
                            continue;
                        }

                        /************************************************************************/
                        /* 处理当前的高优先级车辆H的第一个临界区                                                                     */
                        /************************************************************************/
                        //按照到达某一临界区的先后顺序，进行排序，最终将到达时间在当前高优先车辆H之前的车辆存储至VehicleMng中
                        this.CreateVehicleMng(High_CSID, High_ID, 0);


                        int index_tmp = this.GetPosInVehicleMng(High_ID);            //获取ID车辆在VehicleMng中的位置
                        //xcxc High_stmp是HighPriorVehiclesSorted中临时变量、index_tmp是High_stmp在VehicleMng中的位置
                        int mark = this.ProcFirstCS(High_stmp, Priority, index_tmp);

                        //根据mark的值，进行所有车的判断，
                        //此时若有高优先级车辆不会延迟，那么就遍历新返回的高优先级车辆之前的车辆，若没有，则将遍历当前判断车辆之前的车辆
                        //此时index所指向的车辆将是当前判断的高优先级车辆，或者是当前提升优先级的原高优先级车辆的位置
                        if (mark != -1)
                        {//说明此时返回了可以被提升的高优先级车辆，或者当前高优先级不会被延迟
                            High_Index = mark;

                            //xc Mark是可以被提升优先级车在VehicleMng中的排序
                            if (this.VehicleMng.ElementAt(mark).Value.Priority != this.PriorType
                                && this.VehicleMng.ElementAt(index_tmp).Value.LaneID == this.VehicleMng.ElementAt(mark).Value.LaneID)
                            {
                                this.RaisePriority(this.VehicleMng.ElementAt(mark).Value.ID, this.PriorType, Intersection);
                            }
                        }
                        else
                        {//说明此时判断的High_ID不能被授权，且无法返回一个不会被延迟的高优先级车辆
                            High_Index = this.VehicleMng.Count - 1;
                        }

                        /************************************************************************/
                        /* 按照步骤1所返回的高优先级车辆H，遍历VehicleMng中到达时间在其之前(包括H)的车辆V，
                         * 判断其前需、后需临界区是否可以被授权
                         */
                        /************************************************************************/
                        this.ProcLeftCSs(High_Index, High_CSID);

                        //此时遍历到达时间在车辆H之前(包括H)的车辆，进行授权判断
                        NewAddHighVehiclesLst.Clear();
                        NewAddHighVehiclesLst = this.AuthorizePriorVehicles(Intersection);

                        bool mark1 = false;
                        for (int k = 0; k < NewAddHighVehiclesLst.Count; k++)
                        {

                            string ID = NewAddHighVehiclesLst[k];
                            SuperVehicleManager stmp1 = this.FindPosInLaneQueue(ID);
                            //增加新的高优先级车辆
                            if (this.HighPriorVehicle.Contains(ID) == false)
                            {
                                this.HighPriorVehicle.Add(ID);
                                mark1 = true;
                                IsPriorQueueChanged = true;
                            }
                        }
                        for (int k = 0; k < 5; k++)
                        {
                            this.CanBeAuthorized[k].Clear();
                        }
                        if (mark1)
                        {
                            break;
                        }
                    }
                }
            }
            //当高优先级车辆授权算法结束之后，如果仍旧有临界区的存在，那么此时需要将其授权给其他的车道的车辆？？
            //但是可能回到高优先级车辆的延迟，所以不予考虑
        }

        //获取授权信息
        public Dictionary<int, string> GetAuthorize_CS_to_Vehicle()
        {
            return this.Authorize_CS_to_Vehicle;
        }


        //根据ID在全局的VehicleList中找到Vehicle
        public static Vehicle FindVehicleByID(string ID)
        {
            Vehicle Vehicle = new Vehicle();
            for (int i = 0; i < MainData.Map.GlobalVehicleList.Count(); i++)
            {
                Vehicle veh = MainData.Map.GlobalVehicleList[i];
                if (veh.Get_ID() == ID)
                {
                    Vehicle = veh;
                }
            }
            return Vehicle;
        }
    }
}
